#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <stdio.h>
#include <sys/time.h>   //      gettimeofday
#include <sys/stat.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <errno.h>
#include <math.h>
#include<sys/time.h>

#include "p2v.h"
#include "typedef.h"
#include "udp_get_bitrate.h"
#include "serial.h"
#include "uartinf.h"
#include "confile.h"
#include "BKT.h"
#include "d_png.h"
#include "params.h"

extern u32 g_pLCDBuffer;
extern void LCDDisplayASCII8X16(u32 x0, u32 y0, u32 ForeColor, char * s);
extern void LCDDisplayASCII24x56(u32 x0, u32 y0, u32 ForeColor, char * s);
extern void LCDDisplayASCII24x36(u32 x0, u32 y0, u32 ForeColor, char * s);

#define RED     (0xFFFF0000)
#define BLUE    (0xFF0000FF)
#define GREEN   (0xFF00FF00)
#define YELLOW  (0xFFFFFF00)
#define BLACK   (0xFF000000)
#define GREY   (0xFF5C5C5C)
#define WHITE (0xFFFFFFFF)
#define ORANGE (0xFFFF7200)
#define INDIGO (0xFF00FCFC)
#define CUSTOM_COLOR0 (0xFF0C9AF2)

#define BG_COLOR 0xff0000ff

//是否使用COM信号模拟CAN信号调试
#define IS_COM_TO_CAN_SIM 0
//是否在开始初始化配置,或者每次画线都会初始化
#define IS_INITCONFIG_AT_START 1
//是否使用文本测试,否即使用CAN
#define IS_USE_TEST_TESTING 0
//允许接收到的最大距离
#define DISTANCE_MAX 999

//距离参考线颜色
uint32_t dis_color_0m = RED;
uint32_t dis_color_1m = ORANGE;
uint32_t dis_color_3m = YELLOW;
uint32_t dis_color_5m = INDIGO;
//前方车轮下轨迹线颜色
uint32_t front_lower_color = BLUE;
//前方车轮上轨迹线颜色
uint32_t front_upper_color = GREEN;

//是否显示轨迹线
// #define ISSHOWTRACKLINE 1
int ISSHOWTRACKLINE = 1;
//是否显示距离
// #define ISSHOWDISTANCE 1
int ISSHOWDISTANCE = 1;
//是否显示后轨迹线
// #define ISSHOWTRACKLINE_AFTER 1
int ISSHOWTRACKLINE_AFTER = 0;
//是否显示铲斗姿态
// #define ISSHOWBUCKET 1
int ISSHOWBUCKET = 0;
//是否根据动臂位置显示轨迹线形态,如"1"则当抬臂时隐藏轨迹线1-7m,只显示0.5-1m处轨迹线
#define ISSHOWONTHEPOSOFTHEBOOM 1
//是否隐藏1m外的线,需要上一项开启
int ISHIDELINESBEYOND1M = 0;
//是否只显示0M~7M之间的线
#define ISSHOWONLY07 1
//是否防止闪烁
#define ISNOANTIFLASHING 1
//是否输出参数文本
#define IS_PRINT_PARAMS_TEXT 0
//是否按照实际7M线画横线,否则按照车辆转弯圆心与外圆7M处连线画交点
#define IS_DRAW_ACTUAL_7M_HOR_LINE 0
//辅助坐标系示意
#define IS_SCHEMATIC_DIAGRAM_OF_AUXILIARY_COORDINATE_SYS 0
//是否画车轮上轨迹线
#define IS_DRAW_TRAJECTORY_ON_THE_WHEEL 0
//是否根据轨迹线末端画立体辅助框
#define IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX 0
//是否在屏幕上输出显示时间
#define IS_SHOW_SCREEN_TIME 0
//是否增加3M延长线
#define IS_ADD_3M_EXTENSION_LINE 1

//是否画固定辅助横线
#define IS_DRAW_FIXED_AUXILIARY_HORIZONTAL_LINE 0

//是否显示左右两边柳工logo
#define IS_SHOW_LIUGONG_LOGO 0

// 帧率-刷新一次等待时间/微秒,默认5000
int frameRate = 5000;

uint16_t x_res = 1920;
uint16_t y_res = 1080;

float rssi;
float rssi1;
float rssi2;

uint32_t *working_fb;

int *switch_mode;
int pre_switch_mode;
int switch_mode_pre=0;

int num = 300;

double scale = 0.01;

//角度偏移,统一偏移量
double reviseAngle;

//距离显示最小值与最大值
double distance_min = 0.5, distance_max = 15.0;

int distance_clear = 1;
float distance_f = DISTANCE_MAX;

//透视计算等矩阵变量
double matrixTrans[16];
double matrixRotZ[16];
double matrixRotX[16];
double matrixRotY[16];
double matrixForward[16];
double matrixAngle[16];
double matrixBack[16];

#if IS_SCHEMATIC_DIAGRAM_OF_AUXILIARY_COORDINATE_SYS
//辅助坐标系示意像素点
double leftTopPoint[2];
double rightTopPoint[2];
double leftBottomPoint[2];
double rightBottomPoint[2];
int axisHeight = 30;
#endif

#if IS_DRAW_TRAJECTORY_ON_THE_WHEEL || IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
//角度偏移,统一偏移量
double tireSize = 14.4;
#endif

#if IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
//左上、右上、左下、右下
double box_points[4][2];
#endif

//透视计算等矩阵变量-后摄像头
double matrixTrans_Hou[16];
double matrixRotZ_Hou[16];
double matrixRotX_Hou[16];
double matrixRotY_Hou[16];
double matrixForward_Hou[16];
double matrixAngle_Hou[16];
double matrixBack_Hou[16];

struct display_param{
          int FREQ;
	  int BW;
	  int POWER;
	  int bitrate;
          float RSSI;
          float RSSI1;
          float RSSI2;
};

long getTime()
{
        struct timeval time_now = {0};
        long time_sec = 0;//秒
        long time_mil = 0;//1毫秒 = 1秒/1000 
        // long time_mic = 0;//1微秒 = 1毫秒/1000

        gettimeofday(&time_now,NULL);
        time_sec = time_now.tv_sec;
        time_mil = time_sec * 1000 + time_now.tv_usec/1000;
        // time_mic = time_now.tv_sec * 1000 * 1000 + time_now.tv_usec;

        // return time_sec;
        return time_mil;
        // return time_mic;
}

void printTime()
{
        // printf("second %ld\n",time_sec);
        printf("millisecond %ld\n", getTime());
        // printf("microsecond %ld\n",time_mic);
}

int *switch_memory_open(void) {
        char buf[128];
        int fd;
        sprintf(buf, "/switch_mode");
        fd = shm_open(buf, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR);
        if(fd < 0) { perror("shm_open"); exit(1); }
        if (ftruncate(fd, 4096) == -1) { perror("ftruncate"); exit(1); }
        int *retval = mmap(NULL, 4096, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
        return retval;
}

static void fill_bmp(int fd, uint32_t *buf, uint32_t alpha, int width, int height)
{
    int i, j;
    unsigned char tmp_buf[54];
    unsigned char *bmp_line;

    uint32_t *p = buf;

    printf("++fill_bmp\n");
    unsigned char *bmp_buf  = (unsigned char *)malloc(width*height*3);
    if(bmp_buf == NULL)
        return;

    read(fd, tmp_buf, 54); //skip bmp hdr, TODO,parse hdr
    i = read(fd, bmp_buf, width*height*3);
    if(i != width*height*3) {
        printf("read bmp file fail\n");
        return;
    }
    for(i=0; i<height; i++) {
        bmp_line = bmp_buf + ((height-1-i) * width * 3);
        for(j=0;j<width;j++) {
               *p = (alpha<<24) | (bmp_line[j*3]) |  (bmp_line[j*3+1]<<8) |  ((bmp_line[j*3+2]<<16));
           p++;
        }
    }
    printf("--fill_bmp, %d, %d\n", i, j);
    free(bmp_buf);
}

int fill_bmp_base(int fd, uint32_t *buf,int x0, int y0, int width, int height)
{
        printf("fd : %d, x0 : %d, y0 : %d, width : %d, height : %d\n", fd, x0, y0, width, height);
        int i, j;
        unsigned char tmp_buf[54];
        unsigned char *bmp_line;

        uint32_t *p = buf;

        if((x0 + width > 1920)  || (y0 + height) > 1080){
                printf("Please chech pic size\n");
                return -1;
        }

        printf("++fill_bmp\n");
        unsigned char *bmp_buf  = (unsigned char *)malloc(width*height*3);
        if(bmp_buf == NULL)
                return;

        read(fd, tmp_buf, 54); //skip bmp hdr, TODO,parse hdr
    


    	i = read(fd, bmp_buf, width*height*3);
        if(i != width*height*3) {
                printf("read bmp file fail\n");
                return;
        }

        p= p+ y0*1920;

        for(i=0; i<height; i++) {
                bmp_line = bmp_buf + ((height-1-i) * width * 3);
                p= p + x0 ;
                for(j=0;j<width;j++) {
                        *p = (0xFF<<24) | (bmp_line[j*3]) |  (bmp_line[j*3+1]<<8) |  ((bmp_line[j*3+2]<<16));

                        p++;
                }
                p=p+(1920-x0-width);
        }
        printf("--fill_bmp, %d, %d\n", i, j);
        free(bmp_buf);
}

int fill_bmp_bgr(int fd, uint32_t *buf,int x0, int y0, int width, int height)
{
        int i, j;
        unsigned char tmp_buf[54];
        unsigned char *bmp_line;

        uint32_t *p = buf;

        if((x0 + width > 1920)  || (y0 + height) > 1080){
                printf("Please chech pic size\n");
                return -1;
        }

        printf("++fill_bmp\n");
        unsigned char *bmp_buf  = (unsigned char *)malloc(width*height*3);
        if(bmp_buf == NULL)
                return;

        read(fd, tmp_buf, 54); //skip bmp hdr, TODO,parse hdr
        i = read(fd, bmp_buf, width*height*3);
        if(i != width*height*3) {
                printf("read bmp file fail\n");
                return;
        }

        p= p+ y0*1920;

        for(i=0; i<height; i++) {
                bmp_line = bmp_buf + ((height-1-i) * width * 3);
                p= p + x0 ;
                for(j=0;j<width;j++) {
                        if(*p == 0xFFFFFFFF)
                                *p = (0xFF<<24) | (bmp_line[j*3]) |  (bmp_line[j*3+1]<<8) |  ((bmp_line[j*3+2]<<16));

                        p++;
                }
                p=p+(1920-x0-width);
        }
        printf("--fill_bmp, %d, %d\n", i, j);
        free(bmp_buf);
}



#if 1
int bgr_rect_x=0;
int bgr_rect_y=0;
int bkt_rect_x=0;
int bkt_rect_y=0;
#endif

int base_x;
int base_y;
int base_w;
int base_h;

void INIT_BASE_IMAGE(){

	int logo_base_fd =  open("/tmp/base.bmp", O_RDONLY, S_IRUSR);

	fill_bmp_base(logo_base_fd, (uint32_t *)(working_fb),  base_x, base_y , base_w , base_h);

	close(logo_base_fd);
}

void fill_text_area(int x, int y, int width, int height, uint32_t color )
{
        for (int i = x; i < x + width; i ++)
        {
                for (int j = y; j < y + height; j++)
                {
                        SetPixelOS((int16_t)(i), (int16_t)(j), color);
                }
        }
}

int GetZZJPngOffsetX(float distance)
{
        if(distance < distance_min)
        {
                distance = distance_min;
        }
        if(distance > distance_max)
        {
                distance = distance_max;
        }
        //15~0.5m
        //0~-197px
        //(15,0),(0.5,-197)
        double k = (-197.0 - 0.0) / (distance_min - distance_max);
        double c = distance_min - distance_max * k;
        double offsetX_D = k * distance + c;
        return (int)offsetX_D;
}

void ShowCDZTDistanceValue(float distance)
{
        //(225,330)~(130,330)
        //15~0.5m
        //(15,225)~(0.5,130)
        double k = (130.0 - 225.0) / (distance_min - distance_max);
        double c = 225.0 - distance_max * k;
        double offsetX_Font = k * distance + c;
        double offsetY_Font = 330.0;
        // blt_clr(fb_base0, cameraParams0.axisX - 30 - curAngle * 6, screenParams0.viewMaxY - 40, cameraParams0.axisX - 30 - curAngle * 6 + 100, screenParams0.viewMaxY - 40 + 50);
        if (distance > distance_max)
        {
                char disStr[20];
                sprintf(disStr, ">%.1fM", distance_max);
                offsetX_Font = k * distance_max + c;
                LCDDisplayASCII8X16(base_x + offsetX_Font, base_y + offsetY_Font, GREEN, disStr);
        }
        else
        {
                char disStr[20];
                sprintf(disStr, "%.1fM", distance);
                LCDDisplayASCII8X16(base_x + offsetX_Font, base_y + offsetY_Font, RED, disStr);
        }
}

void DRAW_Base_Bkt_Bgr(int index_bkt,int index_bgr, float distance)
{
	unsigned char bkt[30];
	unsigned char bgr[20];

	int y_bkt_offset =0;
	//struct RECT_D base_rect = ;
	struct RECT_D bkt_rect = BKT_INDEX_VALUE[index_bkt].pos_wh;
	struct RECT_D bgr_rect = BGR_INDEX_VALUE[index_bgr].pos_wh;

	sprintf(bkt, "/tmp/img1/%s", BKT_INDEX_VALUE[index_bkt].filename);
        // printf("bkt str len : %d , value = %s\n", strlen(bkt), bkt);
	sprintf(bgr, "/tmp/img1/%s", BGR_INDEX_VALUE[index_bgr].filename);
        // printf("bgr str len : %d , value = %s\n", strlen(bgr), bgr);

        #if 1
        int zzjOffsetX = GetZZJPngOffsetX(distance);
        // printf("bgr:%s\n", bgr);
        struct filename_postion filename[]={
		{ 0, 0, "/tmp/img1/bg.png" },
		{ 0, 0, "/tmp/img1/liaoche.png" },
		{ zzjOffsetX, 0, "/tmp/img1/zzj.png" },
		{ bgr_rect.x + zzjOffsetX, bgr_rect.y, bgr},
		{ bkt_rect.x + zzjOffsetX + bgr_rect.offsetX, bkt_rect.y + bgr_rect.offsetY, bkt }
	};
	png_blend(filename,sizeof(filename)/sizeof(struct filename_postion));
	#endif

	#if 0
	fill_bmp_base(logo_base_fd, (uint32_t *)(working_fb),  base_x, base_y , base_w , base_h);
        // fill_bmp_base(logo_bkt_fd, (uint32_t *)(working_fb),  bkt_rect.x+base_x , bkt_rect.y+base_y - y_bkt_offset , bkt_rect.w, bkt_rect.h);
        fill_bmp_base(logo_bkt_fd, (uint32_t *)(working_fb),  bkt_rect.x+base_x-bkt_rect.offsetX , bkt_rect.y+base_y-bkt_rect.offsetY-bgr_rect.offsetY , bkt_rect.w, bkt_rect.h);
        fill_bmp_bgr(logo_bgr_fd, (uint32_t *)(working_fb),   bgr_rect.x+base_x , bgr_rect.y+base_y , bgr_rect.w, bgr_rect.h);
	#endif
	
	#if 0
	fill_bmp_base(logo_base_fd, (uint32_t *)(working_fb),  base_x, base_y , base_w , base_h);
        fill_bmp_base(logo_bkt_fd, (uint32_t *)(working_fb),  bkt_rect_x+base_x , bkt_rect_y+base_y-y_bkt_offset , bkt_rect.w, bkt_rect.h);
        fill_bmp_bgr(logo_bgr_fd, (uint32_t *)(working_fb),   bgr_rect_x+base_x , bgr_rect_y+base_y , bgr_rect.w, bgr_rect.h);
	#endif

        // close(logo_base_fd);
        // close(logo_bkt_fd);
	// close(logo_bgr_fd);
}

void SetPixelOS(int16_t x, int16_t y, uint32_t color) //set pixel off screen
{
        working_fb[y * x_res + x] = color;
}

void blt_clr(uint32_t *fb, uint16_t x0, uint16_t y0, uint16_t x1, uint16_t y1)
{
        uint32_t *src_addr;
        int x_stride;
        int i;
        int iter = y1 - y0 + 1;

        x_stride = (x1 - x0 + 1) * 4;
        for(i=0; i<iter; i++) {
                src_addr = &fb[(y0 + i)*x_res + x0];
                memset(src_addr, 0x0, x_stride);
        }
}

int IsScreenVector(double vector[], struct ScreenParams sp)
{
        return IsScreenPoint(vector[0], vector[1], sp);
}

int IsScreenPoint(double x, double y, struct ScreenParams sp)
{
        if (x < sp.viewMinX+2 || y < sp.viewMinY || x > sp.viewMaxX || y > sp.viewMaxY)
        {
                return 0;
        }
        return 1;
}

void SetPointPixelOS(double x, double y, uint32_t color) 
{
        int blockLen = 4;
        for (int i=-blockLen/2;i<blockLen/2;i++)
        {
                for (int j=-blockLen/2;j<blockLen/2;j++)
                {
                        SetPixelOS((int16_t)(x+i), (int16_t)(y+j), color);
                }
        }
}

void SetVectorPixelOS(double vec[], uint32_t color) 
{
        SetPointPixelOS(vec[0], vec[1], color);
}

void DrawStraightLine(double vec0[], double vec1[], uint32_t color, struct ScreenParams sp)
{
        //vec0[0],vec0[1]     vec1[0],vec1[1]
        // printf("(%lf, %lf)    (%lf, %lf)\n", vec0[0], vec0[1], vec1[0], vec1[1]);
        if (vec0[0] == vec1[0] && vec0[1] == vec1[1])
        {
                return;
        }
        else if (vec0[0] == vec1[0])
        {
                double max = (vec0[1] > vec1[1] ? vec0[1] : vec1[1]);
                double min = (vec0[1] < vec1[1] ? vec0[1] : vec1[1]);
                for (double i = min; i < max; i ++)
                {
                        if (IsScreenPoint(vec0[0], i, sp))
                        {
                                SetPointPixelOS(vec0[0], i, color);
                        }
                        
                }
        }
        else if (vec0[1] == vec1[1])
        {
                double max = (vec0[0] > vec1[0] ? vec0[0] : vec1[0]);
                double min = (vec0[0] < vec1[0] ? vec0[0] : vec1[0]);
                for (double i = min; i < max; i ++)
                {
                        if (IsScreenPoint(i, vec0[1], sp))
                        {
                                SetPointPixelOS(i, vec0[1], color);
                        }
                }
        }
        else 
        {
                double k = (vec1[1] - vec0[1]) / (vec1[0] - vec0[0]);
                double c = vec0[1] - k * vec0[0];
                
                double max = (vec0[0] > vec1[0] ? vec0[0] : vec1[0]);
                double min = (vec0[0] < vec1[0] ? vec0[0] : vec1[0]);

                double step = sqrt(1 / (1 + k * k));
                // step = (step > 0.2 ? step : 0.2);

                for (double i = min; i < max; i += step)
                {
                        if (IsScreenPoint(i, k * i + c, sp))
                        {
                                SetPointPixelOS(i, k * i + c, color);
                        }
                }
        }
}

double cot(double radian)
{
        return 1/tan(radian);
}

double deg2Rad(double angle) {
        return angle / 180 * M_PI;
}

//设置屏幕哪条横线是否显示
void set_screen_line_parameter()
{
        screenParams0. ISSHOW7MLine = 1;
        screenParams0. ISSHOW5MLine = 0;
        screenParams0. ISSHOW3MLine = 1;
        screenParams0. ISSHOW1MLine = 0;
        screenParams0. ISSHOW0MLine = 1;

        screenParams1. ISSHOW7MLine = 0;
        screenParams1. ISSHOW5MLine = 0;
        screenParams1. ISSHOW3MLine = 1;
        screenParams1. ISSHOW1MLine = 0;
        screenParams1. ISSHOW0MLine = 0;
}

//获取参数(单位转换)
int get_parameter()
{
        if(confile_init()) {
                printf("config file parse filed!\n");
                return -1;
        }
    	num     =  get_conf_int("NUM", 300);
    	scale   =  get_conf_float("SCALE", 0.01);

	baseParams.a1 =  get_conf_float("A1", 3450);
	baseParams.a2 =  get_conf_float("A2", 3450);
	baseParams.b1 =  get_conf_float("B1", 1660);
	baseParams.b2 =  get_conf_float("B2", 1790);
	baseParams.c1 =  get_conf_float("C1", 2150);
	baseParams.c2 =  get_conf_float("C2", 2150);
	baseParams.d1 =  get_conf_float("D1", 600);
	baseParams.d2 =  get_conf_float("D2", 600);
	baseParams.e =  get_conf_float("E", 3000);
	baseParams.f =  get_conf_float("F", 2976);
	baseParams.rw2c =  get_conf_float("RW2C", 3424);
	baseParams.rf2c =  get_conf_float("RF2C", 5053);
        
        cameraParams0.location = 0;
        cameraParams0.guideline7 = get_conf_float("Guideline7", 9200);
        cameraParams0.guideline5 = get_conf_float("Guideline5", 7250);
	cameraParams0.guideline3       =  get_conf_float("Guideline3", 5300);
	cameraParams0.guideline1       =  get_conf_float("Guideline1", 3420);
	cameraParams0.guideline0       =  get_conf_float("Guideline0", 2900);
	cameraParams0.hw       =  get_conf_float("HW",1200);
        cameraParams0.mw = get_conf_float("MW", 6000);
        
	baseParams.g =  get_conf_float("G", 510);
	cameraParams0.high =  get_conf_float("H", 3650);
	cameraParams0.left       =  get_conf_float("Left", 0);
	cameraParams0.forward       =  get_conf_float("Forward", 0);
	cameraParams0.xAngle =  get_conf_float("XAngle", 15);
        cameraParams0.zAngle = get_conf_float("ZAngle", 0);
        cameraParams0.yAngle = get_conf_float("YAngle", 0);
	cameraParams0.width =  get_conf_float("WIDTH",1920);
	cameraParams0.height =  get_conf_float("HEIGHT",1080);

	cameraParams0.IntrinsicParameters0 =  get_conf_float("IntrinsicParameters0", 559.73878845f);
	cameraParams0.IntrinsicParameters1 =  get_conf_float("IntrinsicParameters1", 0);
	cameraParams0.IntrinsicParameters2 =  get_conf_float("IntrinsicParameters2", 948.09581172f);
	cameraParams0.IntrinsicParameters3 =  get_conf_float("IntrinsicParameters3", 0);
	cameraParams0.IntrinsicParameters4 =  get_conf_float("IntrinsicParameters4", 561.68193815f);
	cameraParams0.IntrinsicParameters5 =  get_conf_float("IntrinsicParameters5", 515.98773609f);

	screenParams0.offsetX =  get_conf_float("OFFSETX",640);
	screenParams0.offsetY =  get_conf_float("OFFSETY",360);
	screenParams0.offsetScale =  get_conf_float("OFFSETSCALE",3);
	screenParams0.viewMinY    =  get_conf_float("ViewMinY",360);
	screenParams0.viewMaxY    =  get_conf_float("ViewMaxY",720);
	screenParams0.viewMinX    =  get_conf_float("ViewMinX",640);
	screenParams0.viewMaxX    =  get_conf_float("ViewMaxX",1280);

    	distortionParams.isDistortion     =  get_conf_int("IsDistortion", 0);
	distortionParams.k1    =  get_conf_float("K1", 0);
	distortionParams.k2    =  get_conf_float("K2", 0);
	distortionParams.k3    =  get_conf_float("K3",0);
	distortionParams.p1    =  get_conf_float("P1", 0);
	distortionParams.p2    =  get_conf_float("P2", 0);
	distortionParams.s1    =  get_conf_float("S1", 0);
	distortionParams.s2    =  get_conf_float("S2", 0);

	// pointX0    =  get_conf_float("PointX0",0);
	// pointY0    =  get_conf_float("PointY0",0);
	// pointX1    =  get_conf_float("PointX1",0);
	// pointY1    =  get_conf_float("PointY1",0);
	// pointX2    =  get_conf_float("PointX2",0);
	// pointY2    =  get_conf_float("PointY2",0);
	// pointX3    =  get_conf_float("PointX3",0);
	// pointY3    =  get_conf_float("PointY3",0);
	// offsetPointX    =  get_conf_float("OffsetPointX",0);
	// offsetPointY    =  get_conf_float("OffsetPointY",0);
	// offsetPointScale    =  get_conf_float("OffsetPointScale",0);

        cameraParams0.zeroPointX0    =  get_conf_float("ZeroPointX0",0);
	cameraParams0.zeroPointY0    =  get_conf_float("ZeroPointY0",0);
	cameraParams0.zeroPointX1    =  get_conf_float("ZeroPointX1",0);
	cameraParams0.zeroPointY1    =  get_conf_float("ZeroPointY1",0);
	cameraParams0.zeroPointX2    =  get_conf_float("ZeroPointX2",0);
	cameraParams0.zeroPointY2    =  get_conf_float("ZeroPointY2",0);
	cameraParams0.zeroPointX3    =  get_conf_float("ZeroPointX3",0);
	cameraParams0.zeroPointY3    =  get_conf_float("ZeroPointY3",0);
	cameraParams0.guideline0PointY    =  get_conf_float("Guideline0PointY",0);
	cameraParams0.guideline1PointY    =  get_conf_float("Guideline1PointY",0);
	cameraParams0.guideline3PointY    =  get_conf_float("Guideline3PointY",0);
	cameraParams0.guideline5PointY    =  get_conf_float("Guideline5PointY",0);
	cameraParams0.guideline7PointY    =  get_conf_float("Guideline7PointY",0);

        cameraParams0.angleThreshold =  get_conf_float("AngleThreshold",0);
        reviseAngle = get_conf_float("ReviseAngle",0);
        cameraParams0.axisX =  get_conf_float("AxisX",0);
	cameraParams0.axisXOffsetX0     =  get_conf_float("AxisXOffsetX0",0);
	cameraParams0.axisXOffsetX1     =  get_conf_float("AxisXOffsetX1",0);
        cameraParams0.showAngle = get_conf_float("ShowAngle", 20);
		
	base_x          = get_conf_int("X_BASE",1280);
	base_y          = get_conf_int("Y_BASE",0);
	base_w          = get_conf_int("X_BASE_W",640);
	base_h          = get_conf_int("Y_BASE_H",360);

        distance_min = get_conf_float("Distance_Min", 0.5);
        distance_max = get_conf_float("Distance_Max", 15);
        
        //后摄像头参数
        cameraParams1.location = 1;
	cameraParams1.high =  get_conf_float("High_Hou", 3650);
	cameraParams1.left       =  get_conf_float("Left_Hou", 0);
	cameraParams1.forward       =  get_conf_float("Forward_Hou", 0);
	cameraParams1.xAngle =  get_conf_float("XAngle_Hou", 15);
        cameraParams1.zAngle = get_conf_float("ZAngle_Hou", 0);
        cameraParams1.yAngle = get_conf_float("YAngle_Hou", 0);
	cameraParams1.width =  get_conf_float("WIDTH_Hou",1920);
	cameraParams1.height =  get_conf_float("HEIGHT_Hou",1080);
        cameraParams1.guideline7 = get_conf_float("Guideline7_Hou", 9200);
        cameraParams1.guideline5 = get_conf_float("Guideline5_Hou", 7250);
	cameraParams1.guideline3       =  get_conf_float("Guideline3_Hou", 5300);
	cameraParams1.guideline1       =  get_conf_float("Guideline1_Hou", 3420);
	cameraParams1.guideline0       =  get_conf_float("Guideline0_Hou", 2900);
        cameraParams1.zeroPointX0    =  get_conf_float("ZeroPointX0_Hou",0);
	cameraParams1.zeroPointY0    =  get_conf_float("ZeroPointY0_Hou",0);
	cameraParams1.zeroPointX1    =  get_conf_float("ZeroPointX1_Hou",0);
	cameraParams1.zeroPointY1    =  get_conf_float("ZeroPointY1_Hou",0);
	cameraParams1.zeroPointX2    =  get_conf_float("ZeroPointX2_Hou",0);
	cameraParams1.zeroPointY2    =  get_conf_float("ZeroPointY2_Hou",0);
	cameraParams1.zeroPointX3    =  get_conf_float("ZeroPointX3_Hou",0);
	cameraParams1.zeroPointY3    =  get_conf_float("ZeroPointY3_Hou",0);
	cameraParams1.guideline0PointY    =  get_conf_float("Guideline0PointY_Hou",0);
	cameraParams1.guideline1PointY    =  get_conf_float("Guideline1PointY_Hou",0);
	cameraParams1.guideline3PointY    =  get_conf_float("Guideline3PointY_Hou",0);
	cameraParams1.guideline5PointY    =  get_conf_float("Guideline5PointY_Hou",0);
	cameraParams1.guideline7PointY    =  get_conf_float("Guideline7PointY_Hou",0);
        cameraParams1.angleThreshold =  get_conf_float("AngleThreshold_Hou",0);

	cameraParams1.IntrinsicParameters0 =  get_conf_float("IntrinsicParameters_Hou0", 559.73878845f);
	cameraParams1.IntrinsicParameters1 =  get_conf_float("IntrinsicParameters_Hou1", 0);
	cameraParams1.IntrinsicParameters2 =  get_conf_float("IntrinsicParameters_Hou2", 948.09581172f);
	cameraParams1.IntrinsicParameters3 =  get_conf_float("IntrinsicParameters_Hou3", 0);
	cameraParams1.IntrinsicParameters4 =  get_conf_float("IntrinsicParameters_Hou4", 561.68193815f);
	cameraParams1.IntrinsicParameters5 =  get_conf_float("IntrinsicParameters_Hou5", 515.98773609f);

	screenParams1.offsetX =  get_conf_float("OFFSETX_Hou",640);
	screenParams1.offsetY =  get_conf_float("OFFSETY_Hou",360);
	screenParams1.offsetScale =  get_conf_float("OFFSETSCALE_Hou",3);
	screenParams1.viewMinY    =  get_conf_float("ViewMinY_Hou",360);
	screenParams1.viewMaxY    =  get_conf_float("ViewMaxY_Hou",720);
	screenParams1.viewMinX    =  get_conf_float("ViewMinX_Hou",640);
	screenParams1.viewMaxX    =  get_conf_float("ViewMaxX_Hou",1280);
        
	testParams.testIsShowTrackline    =  get_conf_int("TestIsShowTrackline", 1);
	testParams.testAngle    =  get_conf_float("TestAngle", 0);

	#if IS_PRINT_PARAMS_TEXT
	printf("num     is     %d\n",num);
	printf("scale   is     %f\n",scale);		

	printf("baseParams.a1   is     %f\n",baseParams.a1);		
	printf("baseParams.a2   is     %f\n",baseParams.a2);		
	printf("baseParams.b1   is     %f\n",baseParams.b1);		
	printf("baseParams.b2   is     %f\n",baseParams.b2);		
	printf("baseParams.c1   is     %f\n",baseParams.c1);				
	printf("baseParams.c2   is     %f\n",baseParams.c2);		
	printf("baseParams.d1   is     %f\n",baseParams.d1);		
	printf("baseParams.d2   is     %f\n",baseParams.d2);	
	printf("baseParams.e   is     %f\n",baseParams.e);
	printf("baseParams.f   is     %f\n",baseParams.f);
	printf("baseParams.rw2c   is     %f\n",baseParams.rw2c);
	printf("baseParams.rf2c   is     %f\n",baseParams.rf2c);
	printf("cameraParams0.guideline7   is     %f\n",cameraParams0.guideline7);
	printf("cameraParams0.guideline5   is     %f\n",cameraParams0.guideline5);
	printf("cameraParams0.guideline3   is     %f\n",cameraParams0.guideline3);	
	printf("cameraParams0.guideline1   is     %f\n",cameraParams0.guideline1);	
	printf("cameraParams0.guideline0   is     %f\n",cameraParams0.guideline0);
	printf("cameraParams0.hw   is     %f\n",cameraParams0.hw);	
	printf("cameraParams0.mw   is     %f\n",cameraParams0.mw);	

	printf("baseParams.g   is     %f\n",baseParams.g);		
	printf("cameraParams0.high   is     %f\n",cameraParams0.high);	
	printf("cameraParams0.left   is     %f\n",cameraParams0.left);		
	printf("cameraParams0.forward   is     %f\n",cameraParams0.forward);
	printf("cameraParams0.xAngle   is     %f\n", cameraParams0.xAngle);		
	printf("cameraParams0.zAngle   is     %f\n", cameraParams0.zAngle);	
	printf("cameraParams0.yAngle   is     %f\n", cameraParams0.yAngle);	
	printf("cameraParams0.width   is     %f\n",cameraParams0.width);		
	printf("cameraParams0.height   is     %f\n",cameraParams0.height);

	printf("screenParams0.offsetX   is     %f\n",screenParams0.offsetX);
	printf("screenParams0.offsetY   is     %f\n",screenParams0.offsetY);		
	printf("screenParams0.offsetScale   is     %f\n",screenParams0.offsetScale);		
	printf("screenParams0.viewMinY   is     %f\n",screenParams0.viewMinY);		
	printf("screenParams0.viewMaxY   is     %f\n",screenParams0.viewMaxY);		
	printf("screenParams0.viewMinX   is     %f\n",screenParams0.viewMinX);		
	printf("screenParams0.viewMaxX   is     %f\n",screenParams0.viewMaxX);

	printf("distortionParams.isDistortion     is     %d\n",distortionParams.isDistortion);
	printf("distortionParams.k1   is     %f\n",distortionParams.k1);		
	printf("distortionParams.k2   is     %f\n",distortionParams.k2);		
	printf("distortionParams.k3   is     %f\n",distortionParams.k3);
	printf("distortionParams.p1   is     %f\n",distortionParams.p1);		
	printf("distortionParams.p2   is     %f\n",distortionParams.p2);
	printf("distortionParams.s1   is     %f\n",distortionParams.s1);		
	printf("distortionParams.s2   is     %f\n",distortionParams.s2);
   	
	// printf("pointX0   is     %f\n",pointX0);		
	// printf("pointY0   is     %f\n",pointY0);		
	// printf("pointX1   is     %f\n",pointX1);
	// printf("pointY1   is     %f\n",pointY1);		
	// printf("pointX2   is     %f\n",pointX2);
	// printf("pointY2   is     %f\n",pointY2);		
	// printf("pointX3   is     %f\n",pointX3);	
	// printf("pointY3   is     %f\n",pointY3);
	// printf("pointY2   is     %f\n",pointY2);		
	// printf("offsetPointX   is     %f\n",offsetPointX);	
	// printf("offsetPointY   is     %f\n",offsetPointY);
	// printf("offsetPointScale   is     %f\n",offsetPointScale);

        printf("cameraParams0.zeroPointX0   is     %f\n",cameraParams0.zeroPointX0);		
	printf("cameraParams0.zeroPointY0   is     %f\n",cameraParams0.zeroPointY0);		
	printf("cameraParams0.zeroPointX1   is     %f\n",cameraParams0.zeroPointX1);
	printf("cameraParams0.zeroPointY1   is     %f\n",cameraParams0.zeroPointY1);		
	printf("cameraParams0.zeroPointX2   is     %f\n",cameraParams0.zeroPointX2);
	printf("cameraParams0.zeroPointY2   is     %f\n",cameraParams0.zeroPointY2);		
	printf("cameraParams0.zeroPointX3   is     %f\n",cameraParams0.zeroPointX3);	
	printf("cameraParams0.zeroPointY3   is     %f\n",cameraParams0.zeroPointY3);
	printf("pointY2   is     %f\n",pointY2);		
	printf("cameraParams0.guideline0PointY   is     %f\n",cameraParams0.guideline0PointY);
	printf("cameraParams0.guideline1PointY   is     %f\n",cameraParams0.guideline1PointY);
	printf("cameraParams0.guideline3PointY   is     %f\n",cameraParams0.guideline3PointY);	
	printf("cameraParams0.guideline5PointY   is     %f\n",cameraParams0.guideline5PointY);
	printf("cameraParams0.guideline7PointY   is     %f\n",cameraParams0.guideline7PointY);

	printf("cameraParams0.angleThreshold   is     %f\n",cameraParams0.angleThreshold);
	printf("reviseAngle   is     %f\n",reviseAngle);
	printf("cameraParams0.axisX   is     %f\n", cameraParams0.axisX);
	printf("cameraParams0.axisXOffsetX0   is     %f\n", cameraParams0.axisXOffsetX0);
	printf("cameraParams0.axisXOffsetX1   is     %f\n", cameraParams0.axisXOffsetX1);
	printf("cameraParams0.showAngle   is     %f\n", cameraParams0.showAngle);
        
	printf("cameraParams0.IntrinsicParameters0   is     %f\n", cameraParams0.IntrinsicParameters0);
	printf("cameraParams0.IntrinsicParameters1   is     %f\n", cameraParams0.IntrinsicParameters1);
	printf("cameraParams0.IntrinsicParameters2   is     %f\n", cameraParams0.IntrinsicParameters2);
	printf("cameraParams0.IntrinsicParameters3   is     %f\n", cameraParams0.IntrinsicParameters3);
	printf("cameraParams0.IntrinsicParameters4   is     %f\n", cameraParams0.IntrinsicParameters4);
	printf("cameraParams0.IntrinsicParameters5   is     %f\n", cameraParams0.IntrinsicParameters5);
	
	printf("base_x   is     %d\n",base_x);
	printf("base_y   is     %d\n",base_y);
	printf("base_w   is     %d\n",base_w);
	printf("base_h   is     %d\n",base_h);
	printf("distance_min   is     %f\n", distance_min);
	printf("distance_max   is    %f\n", distance_max);
        
        //后摄像头参数
	printf("cameraParams1.high   is     %f\n",cameraParams1.high);
	printf("cameraParams1.left   is     %f\n",cameraParams1.left);
	printf("cameraParams1.forward   is     %f\n",cameraParams1.forward);
	printf("cameraParams1.xAngle    is     %f\n",cameraParams1.xAngle);
        printf("cameraParams1.zAngle   is     %f\n",cameraParams1.zAngle);
        printf("cameraParams1.yAngle   is     %f\n",cameraParams1.yAngle);
	printf("cameraParams1.width   is     %f\n",cameraParams1.width);
	printf("cameraParams1.height   is     %f\n",cameraParams1.height);
        printf("cameraParams1.guideline7   is     %f\n",cameraParams1.guideline7);
        printf("cameraParams1.guideline5   is     %f\n",cameraParams1.guideline5);
	printf("cameraParams1.guideline3   is     %f\n",cameraParams1.guideline3);
	printf("cameraParams1.guideline1   is     %f\n",cameraParams1.guideline1);
	printf("cameraParams1.guideline0   is     %f\n",cameraParams1.guideline0);
        printf("cameraParams1.zeroPointX0   is     %f\n",cameraParams1.zeroPointX0);
	printf("cameraParams1.zeroPointY0   is     %f\n",cameraParams1.zeroPointY0);
	printf("cameraParams1.zeroPointX1   is     %f\n",cameraParams1.zeroPointX1);
	printf("cameraParams1.zeroPointY1   is     %f\n",cameraParams1.zeroPointY1);
	printf("cameraParams1.zeroPointX2   is     %f\n",cameraParams1.zeroPointX2);
	printf("cameraParams1.zeroPointY2   is     %f\n",cameraParams1.zeroPointY2);
	printf("cameraParams1.zeroPointX3   is     %f\n",cameraParams1.zeroPointX3);
	printf("cameraParams1.zeroPointY3   is     %f\n",cameraParams1.zeroPointY3);
	printf("cameraParams1.guideline0PointY   is     %f\n",cameraParams1.guideline0PointY);
	printf("cameraParams1.guideline1PointY    is     %f\n",cameraParams1.guideline1PointY);
	printf("cameraParams1.guideline3PointY   is     %f\n",cameraParams1.guideline3PointY);
	printf("cameraParams1.guideline5PointY   is     %f\n",cameraParams1.guideline5PointY);
	printf("cameraParams1.guideline7PointY   is     %f\n",cameraParams1.guideline7PointY);
	printf("cameraParams1.angleThreshold   is     %f\n",cameraParams1.angleThreshold);
        
	printf("cameraParams1.IntrinsicParameters0   is     %f\n", cameraParams1.IntrinsicParameters0);
	printf("cameraParams1.IntrinsicParameters1   is     %f\n", cameraParams1.IntrinsicParameters1);
	printf("cameraParams1.IntrinsicParameters2   is     %f\n", cameraParams1.IntrinsicParameters2);
	printf("cameraParams1.IntrinsicParameters3   is     %f\n", cameraParams1.IntrinsicParameters3);
	printf("cameraParams1.IntrinsicParameters4   is     %f\n", cameraParams1.IntrinsicParameters4);
	printf("cameraParams1.IntrinsicParameters5   is     %f\n", cameraParams1.IntrinsicParameters5);

	printf("screenParams1.offsetX   is     %f\n",screenParams1.offsetX);
	printf("screenParams1.offsetY   is     %f\n",screenParams1.offsetY);		
	printf("screenParams1.offsetScale   is     %f\n",screenParams1.offsetScale);		
	printf("screenParams1.viewMinY   is     %f\n",screenParams1.viewMinY);		
	printf("screenParams1.viewMaxY   is     %f\n",screenParams1.viewMaxY);		
	printf("screenParams1.viewMinX   is     %f\n",screenParams1.viewMinX);		
	printf("screenParams1.viewMaxX   is     %f\n",screenParams1.viewMaxX);
        
	printf("testParams.testIsShowTrackline   is     %f\n", testParams.testIsShowTrackline);		
	printf("testParams.testAngle   is     %f\n", testParams.testAngle);

	#endif
	return 0;
}

void InitParameter()
{
        if (scale != 0)
        {
                baseParams.a1 *= scale;
                baseParams.a2 *= scale;
                baseParams.b1 *= scale;
                baseParams.b2 *= scale;
                baseParams.c1 *= scale;
                baseParams.c2 *= scale;
                baseParams.d1 *= scale;
                baseParams.d2 *= scale;
                baseParams.e *= scale;
                baseParams.f *= scale;
                baseParams.g *= scale;
                baseParams.rw2c *= scale;
                baseParams.rf2c *= scale;

                cameraParams0.high *= scale;
                cameraParams0.left *= scale;
                cameraParams0.forward *= scale;
                cameraParams0.guideline7 *= scale;
                cameraParams0.guideline5 *= scale;
                cameraParams0.guideline3 *= scale;
                cameraParams0.guideline1 *= scale;
                cameraParams0.guideline0 *= scale;
                cameraParams0.mw *= scale;
                cameraParams0.hw *= scale;
                
                cameraParams1.high *= scale;
                cameraParams1.left *= scale;
                cameraParams1.forward *= scale;
                cameraParams1.guideline7 *= scale;
                cameraParams1.guideline5 *= scale;
                cameraParams1.guideline3 *= scale;
                cameraParams1.guideline1 *= scale;
                cameraParams1.guideline0 *= scale;
                cameraParams1.mw *= scale;
                cameraParams1.hw *= scale;
        }
        screenParams0.viewMaxX = screenParams0.viewMinX + cameraParams0.width / screenParams0.offsetScale;
        screenParams0.viewMaxY = screenParams0.viewMinY + cameraParams0.height / screenParams0.offsetScale;
        if (screenParams0.offsetScale == 1.5)
        {
                cameraParams0.axisX = cameraParams0.axisX / screenParams0.offsetScale;
        }
        
        screenParams1.viewMaxX = screenParams1.viewMinX + cameraParams1.width / screenParams1.offsetScale;
        screenParams1.viewMaxY = screenParams1.viewMinY + cameraParams1.height / screenParams1.offsetScale;
        if (screenParams1.offsetScale == 1.5)
        {
                cameraParams1.axisX = cameraParams1.axisX / screenParams1.offsetScale;
        }
}

double GetAxisXOffsetX(int isMirror)
{
        if (isMirror == 1)
        {
                return cameraParams0.axisXOffsetX0;
        }
        else
        {
                return cameraParams0.axisXOffsetX1;
        }
}

void InitMatrix(double values[], double matrixs[])
{
        for (int i = 0; i < 16; i ++)
        {
                matrixs[i] = values[i];
        }
}

void InitParamMatrix(double angle, int isMirror)
{
        //平移矩阵
        double matrixTransValues[] = {
                1, 0, 0, -cameraParams0.forward,
                0, 1, 0, -cameraParams0.high,
                0, 0, 1, -cameraParams0.left,
                0,0,0,1
                };
        //绕Z轴旋转矩阵
        double valzAngle = deg2Rad(-cameraParams0.zAngle);
        double cosValzAngle = cos(valzAngle);
        double sinValzAngle = sin(valzAngle);
        double matrixRotZValues[] = {
                cosValzAngle,-sinValzAngle,0,0,
                sinValzAngle,cosValzAngle,0,0,
                0,0,1,0,
                0,0,0,1
                };
        //绕X轴旋转矩阵
        double valxAngle = deg2Rad(-cameraParams0.xAngle);
        double cosValxAngle = cos(valxAngle);
        double sinValxAngle = sin(valxAngle);
        double matrixRotXValues[] = {
                1,0,0,0,
                0,cosValxAngle,-sinValxAngle,0,
                0,sinValxAngle,cosValxAngle,0,
                0,0,0,1
                };
        //绕Y轴旋转矩阵
        double valyAngle = deg2Rad(-cameraParams0.yAngle);
        double cosValyAngle = cos(valyAngle);
        double sinValyAngle = sin(valyAngle);
        double matrixRotYValues[] = {
                cosValyAngle,0,sinValyAngle,0,
                0,1,0,0,
                -sinValyAngle,0,cosValyAngle,0,
                0,0,0,1
                };

        //横线旋转先进行向原点平移矩阵
        double matrixForwardValues[] = {
                1,0,0,-baseParams.g,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1
                };
        //横线旋转矩阵
        // double valAngle = deg2Rad(-angle);
        double valAngle = deg2Rad(-angle * isMirror);
        double cosValAngle = cos(valAngle);
        double sinValAngle = sin(valAngle);
        double matrixAngleValues[] = {
                cosValAngle,0,sinValAngle,0,
                0,1,0,0,
                -sinValAngle,0,cosValAngle,0,
                0,0,0,1
                };
        //横线旋转后进行远离原点平移矩阵
        double matrixBackValues[] = {
                1,0,0,baseParams.g,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1
                };

        InitMatrix(matrixTransValues, matrixTrans);
        InitMatrix(matrixRotZValues, matrixRotZ);
        InitMatrix(matrixRotXValues, matrixRotX);
        InitMatrix(matrixRotYValues, matrixRotY);
        InitMatrix(matrixForwardValues, matrixForward);
        InitMatrix(matrixAngleValues, matrixAngle);
        InitMatrix(matrixBackValues, matrixBack);

}

void InitParamMatrix_Hou(double angle, int isMirror)
{
        //平移矩阵
        double matrixTransValues[] = {
                1, 0, 0, -cameraParams1.forward,
                0, 1, 0, -cameraParams1.high,
                0, 0, 1, -cameraParams1.left,
                0,0,0,1
                };
        //绕Z轴旋转矩阵
        double valzAngle = deg2Rad(-cameraParams1.zAngle);
        double cosValzAngle = cos(valzAngle);
        double sinValzAngle = sin(valzAngle);
        double matrixRotZValues[] = {
                cosValzAngle,-sinValzAngle,0,0,
                sinValzAngle,cosValzAngle,0,0,
                0,0,1,0,
                0,0,0,1
                };
        //绕X轴旋转矩阵
        double valxAngle = deg2Rad(-cameraParams1.xAngle);
        double cosValxAngle = cos(valxAngle);
        double sinValxAngle = sin(valxAngle);
        double matrixRotXValues[] = {
                1,0,0,0,
                0,cosValxAngle,-sinValxAngle,0,
                0,sinValxAngle,cosValxAngle,0,
                0,0,0,1
                };
        //绕Y轴旋转矩阵
        double valyAngle = deg2Rad(-cameraParams1.yAngle);
        double cosValyAngle = cos(valyAngle);
        double sinValyAngle = sin(valyAngle);
        double matrixRotYValues[] = {
                cosValyAngle,0,sinValyAngle,0,
                0,1,0,0,
                -sinValyAngle,0,cosValyAngle,0,
                0,0,0,1
                };

        //横线旋转先进行向原点平移矩阵
        double matrixForwardValues[] = {
                1,0,0,-baseParams.g,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1
                };
        //横线旋转矩阵
        // double valAngle = deg2Rad(-angle);
        double valAngle = deg2Rad(-angle * isMirror);
        double cosValAngle = cos(valAngle);
        double sinValAngle = sin(valAngle);
        double matrixAngleValues[] = {
                cosValAngle,0,sinValAngle,0,
                0,1,0,0,
                -sinValAngle,0,cosValAngle,0,
                0,0,0,1
                };
        //横线旋转后进行远离原点平移矩阵
        double matrixBackValues[] = {
                1,0,0,baseParams.g,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1
                };

        InitMatrix(matrixTransValues, matrixTrans_Hou);
        InitMatrix(matrixRotZValues, matrixRotZ_Hou);
        InitMatrix(matrixRotXValues, matrixRotX_Hou);
        InitMatrix(matrixRotYValues, matrixRotY_Hou);
        InitMatrix(matrixForwardValues, matrixForward_Hou);
        InitMatrix(matrixAngleValues, matrixAngle_Hou);
        InitMatrix(matrixBackValues, matrixBack_Hou);
}

void FreeParameter()
{
        
}

void FreeParamMatrix()
{
        // FreeMatrix(matrixTrans);
        // FreeMatrix(matrixRotZ);
        // FreeMatrix(matrixRotX);
        // FreeMatrix(matrixRotY);
        // FreeMatrix(matrixForward);
        // FreeMatrix(matrixAngle);
        // FreeMatrix(matrixBack);
}

//求两点之间的距离
double Distance(double pos0[], double pos1[])
{
       return sqrt((pos0[0] - pos1[0]) *  (pos0[0] - pos1[0]) +  (pos0[2] - pos1[2]) *  (pos0[2] - pos1[2])) ;
}

//求两点的中点
double* GetCenter(double pos0[], double pos1[], double center[])
{
        center[0] = (pos0[0] + pos1[0]) / 2;
        center[2] = (pos0[2] + pos1[2]) / 2;
        // center = {(pos0[0] + pos1[0]) / 2, 0, (pos0[2] + pos1[2]) / 2, 1};      //测试去掉此方法镜像
        return center;
}

//前轴中心轨迹半径(R前中)
double GetR1(double angle) {
       return  (baseParams.b1 / cos(deg2Rad(angle)) + baseParams.a1 - baseParams.b1) / cos(deg2Rad(90 - angle)) - baseParams.b1 * tan(deg2Rad(angle));
}

//后轴中心轨迹半径(R后中)
double GetR2(double angle) {
       return  (baseParams.b2 / cos(deg2Rad(angle)) + baseParams.a2 - baseParams.b2) / cos(deg2Rad(90 - angle)) - baseParams.b2 * tan(deg2Rad(angle));
}

//前铲外侧轨迹方程
double GetQianchanY(double angle, double x, int isMirror, int minusValue)
{
        if (angle == 0)
        {
                return baseParams.f / 2;
        }
        else
        {
                double r = minusValue* isMirror * sqrt(pow((baseParams.f / 2 + GetR1(angle)), 2) + pow(baseParams.e, 2) - pow((x - (baseParams.g + baseParams.b2)), 2)) - isMirror * GetR2(angle);
                if (isnan(r))
                {
                        return 0;
                }
                return r;
        }
}

//前轮内侧转弯轨迹方程
double GetQianlunneiceY(double angle, double x, int isMirror, int minusValue)
{
        if (angle == 0)
        {
                return -baseParams.c1/ 2 - baseParams.d1 / 2;
        }
        else
        {
                return minusValue * isMirror * sqrt(pow(GetR1(angle) - baseParams.c1 / 2 - baseParams.d1 / 2, 2) - pow((x - (baseParams.g + baseParams.b2)), 2)) - isMirror * GetR2(angle);
        }
}

// 后轮内侧轨迹线
double GetHoulunneiceY(double angle,double x,  int isMirror, int minusValue)
{
        if (angle == 0)
        {
            return -baseParams.c2 / 2 - baseParams.d2 / 2;
        }
        else
        {
            return minusValue * isMirror * sqrt(pow(GetR2(angle) - baseParams.c2 / 2 - baseParams.d1 / 2, 2) - pow(x - (baseParams.g + baseParams.b2), 2)) - isMirror * GetR2(angle);
        }
}

// 后轮外侧轨迹线
double GetHoulunwaiceY(double angle, double x, int isMirror, int minusValue)
{
        if (angle == 0)
        {
            return -baseParams.c2 / 2 - baseParams.d2 / 2;
        }
        else
        {
            return minusValue * isMirror * sqrt(pow(GetR2(angle) + baseParams.c2 / 2 + baseParams.d1 / 2, 2) - pow(x - (baseParams.g + baseParams.b2), 2)) - isMirror * GetR2(angle);
        }
}

int IsClipPoint(double vector[])
{
        if ((-vector[3] <= vector[0] && vector[0] <= vector[3]) &&
             (-vector[3] <= vector[1] && vector[1] <= vector[3]) &&
             (-vector[3] <= vector[2] && vector[2] <= vector[3]))
        {
                return 0;
        }
        return 1;
}

//向量右乘矩阵,矩阵必须为方阵,且矩阵的列数必须等于向量的长度,参照U3D源码,重写矩阵与点坐标相乘
double* MultiplyPointEX(double matrix[], double vector[], double result[])
{
        // printf("EX matrix:%lf,%lf,%lf,%lf,\n%lf,%lf,%lf,%lf,\n%lf,%lf,%lf,%lf,\n%lf,%lf,%lf,%lf,\n", matrix[0], matrix[1], matrix[2], matrix[3], matrix[4], matrix[5], matrix[6], matrix[7], matrix[8], matrix[9], matrix[10], matrix[11], matrix[12], matrix[13], matrix[14], matrix[15]);
        result[0] = matrix[0] * vector[0] + matrix[1] * vector[1] + matrix[2] * vector[2] + matrix[3];
        result[1] = matrix[4] * vector[0] + matrix[5] * vector[1] + matrix[6] * vector[2] + matrix[7];
        result[2] = matrix[8] * vector[0] + matrix[9] * vector[1] + matrix[10] * vector[2] + matrix[11];
        result[3] = matrix[12] * vector[0] + matrix[13] * vector[1] + matrix[14] * vector[2] + matrix[15];
		
        result[3] = 1.0f / result[3];
        result[0] *= result[3];
        result[1] *= result[3];
        result[2] *= result[3];
	return result;
}

//矩阵变换:从世界坐标系(World)到相机坐标系(Camera)-后摄像头
//w齐次坐标:方向补0点补1
double* MatrixTransformWToC(double value[], double result[])
{
        double result0[] = {0, 0, 0, 0};
        double *vec0 = MultiplyPointEX(matrixTrans, value, result0);
        double result1[] = {0, 0, 0, 0};
        double *vec1 = MultiplyPointEX(matrixRotY, vec0, result);
        double result2[] = {0, 0, 0, 0};
        double *vec2 = MultiplyPointEX(matrixRotX, vec1, result2);
        double *vec3 = MultiplyPointEX(matrixRotZ, vec2, result);
        return vec3;
}

//矩阵变换:从世界坐标系(World)到相机坐标系(Camera)
//w齐次坐标:方向补0点补1
double* MatrixTransformWToC_Hou(double value[], double result[])
{
        double result0[] = {0, 0, 0, 0};
        double *vec0 = MultiplyPointEX(matrixTrans_Hou, value, result0);
        double result1[] = {0, 0, 0, 0};
        double *vec1 = MultiplyPointEX(matrixRotY_Hou, vec0, result);
        double result2[] = {0, 0, 0, 0};
        double *vec2 = MultiplyPointEX(matrixRotX_Hou, vec1, result2);
        double *vec3 = MultiplyPointEX(matrixRotZ_Hou, vec2, result);
        return vec3;
}

//使用内参矩阵进行相机空间到屏幕空间的变换
double* MatrixTransformCToSByInternalReference(double value[], double result[], struct CameraParams cp)
{
        /// 标准标定板标定数据
        /// Calibration - ZhangZhengyou - Method - master
        
        double valueIR[] = {
                cp.IntrinsicParameters0, cp.IntrinsicParameters1, cp.IntrinsicParameters2,0,
                cp.IntrinsicParameters3, cp.IntrinsicParameters4, cp.IntrinsicParameters5,0,
                0,0,1,0,
                0,0,1,0
                };

        double *screen = MultiplyPointEX(valueIR, value, result);
        return screen;
}
    
//摄像机畸变变换
double* DistortionTransform(double vc[], double result[])
{
        double xx = vc[0] / cameraParams0.width - 0.5f;
        double yy = vc[1] / cameraParams0.height - 0.5f;
        double r2 = sqrt(xx * xx + yy * yy);
        result[0] = xx * (1 + distortionParams.k1 * r2 + distortionParams.k2 * pow(r2, 2) + distortionParams.k3 * pow(r2, 4)) + 2 * distortionParams.p1 * xx * yy + distortionParams.p2 * (r2 + 2 * xx * xx) + distortionParams.s1 * r2;
        result[1] = yy * (1 + distortionParams.k1 * r2 + distortionParams.k2 * pow(r2, 2) + distortionParams.k3 * pow(r2, 4)) + 2 * distortionParams.p2 * xx * yy + distortionParams.p1 * (r2 + 2 * yy * yy) + distortionParams.s2 * r2;
        result[0] += 0.5f;
        result[1] += 0.5f;
        result[0] *= cameraParams0.width;
        result[1] *= cameraParams0.height;

        result[2] = vc[2];
        result[3] = vc[3];
        return result;
}

//矩阵变换:从世界坐标到屏幕-内参
double* MatrixTransformWToS(double value[], double result[], struct CameraParams cp, struct ScreenParams sp, int isHorizontalLine)
{
        double result0[] = {0, 0, 0, 0};
        double *vecWToCam = MatrixTransformWToC(value, result0);
        if (cp.location)
        {
                vecWToCam = MatrixTransformWToC_Hou(value, result0);
        }
        double *vecScreen;
        if (isHorizontalLine == 0 && cp.location == 0 && distortionParams.isDistortion)
        {
                double result1[] = {0, 0, 0, 0};
                double *vecScreenNoDistorion = MatrixTransformCToSByInternalReference(vecWToCam, result1, cp);
                if (isnan(vecScreenNoDistorion[2]))
                {
                        vecScreenNoDistorion[2] = 0;
                }
                vecScreenNoDistorion[3] = -vecScreenNoDistorion[2];
                vecScreen = DistortionTransform(vecScreenNoDistorion, result);
        }
        else
        {
                vecScreen = MatrixTransformCToSByInternalReference(vecWToCam, result, cp);
                if (isnan(vecScreen[2]))
                {
                        vecScreen[2] = 0;
                }
                vecScreen[3] = -vecScreen[2];
        }

        vecScreen[1] = cp.height - vecScreen[1];

        vecScreen[0] = vecScreen[0] / sp.offsetScale;
        vecScreen[1] = vecScreen[1] / sp.offsetScale;
        vecScreen[0] += sp.offsetX;
        vecScreen[1] += sp.offsetY;

        return vecScreen;
}

//矩阵变换:横线绕世界坐标系下Y轴旋转
double* MatrixTransformHorizontalLine(double vector[], double result[])
{
        double result0[] = {0, 0, 0, 0};
        double *vec0 = MultiplyPointEX(matrixForward, vector, result0);
        double result1[] = {0, 0, 0, 0};
        double *vec1 = MultiplyPointEX(matrixAngle, vec0, result1);
        double *vec2 = MultiplyPointEX(matrixBack, vec1, result);
        return vec2;
}

//求直线和圆的交点,取小的值
//z=kx+c
//r*r=(x-a)*(x-a) + (z-b)*(z-b)
double* GetPointLineAndCircle(double k, double c, double a, double b, double r, double result[])
{
        double delta = (2*(c - b)*k - 2*a)*(2*(c - b)*k - 2*a) - 4*(1 + k*k)*(a*a+(c-b)*(c-b)-r*r);
        double x = ((2*a - 2*(c - b)*k) - sqrt(delta))/(2*(1+k*k));
        double z = k*x+c;
        result[0] = x;
        result[2] = z;
        return result;
}

//求直线和圆的交点,取大的值
//z=kx+c
//r*r=(x-a)*(x-a) + (z-b)*(z-b)
double* GetPointLineAndCircle1(double k, double c, double a, double b, double r, double result[])
{
        double delta = (2*(c - b)*k - 2*a)*(2*(c - b)*k - 2*a) - 4*(1 + k*k)*(a*a+(c-b)*(c-b)-r*r);
        double x = ((2*a - 2*(c - b)*k) + sqrt(delta))/(2*(1+k*k));
        double z = k*x+c;
        result[0] = x;
        result[2] = z;
        return result;
}

// 求两个圆固定X下的对应两个点
double* GetFixedXTwoCircleSolutions(double x, double a, double b, double r0, double r1, int isMirror, double result[])
{
        result[0] = b + isMirror * sqrt(r0 * r0 - (x - a) * (x - a));
        result[1] = b + isMirror * sqrt(r1 * r1 - (x - a) * (x - a));
        return result;
}

//直接画直行时0度的直线-固定两点画线
void TrajectoryZeroDegreeLine(struct CameraParams cp, struct ScreenParams sp)
{
        //轨迹线直线方程求解
        double k0 = (cp.zeroPointY0 - cp.zeroPointY1) / (cp.zeroPointX0 - cp.zeroPointX1);
        double c0 = cp.zeroPointY1 - cp.zeroPointX1 * k0;
        double k1 = (cp.zeroPointY2 - cp.zeroPointY3) / (cp.zeroPointX2 - cp.zeroPointX3);
        double c1 = cp.zeroPointY3 - cp.zeroPointX3 * k1;

        double ks[] = { k0, k1 };
        double cs[] = { c0, c1 };

        //横线
        double guideline0PointX0 = (cp.guideline0PointY - cs[0]) / ks[0];
        double guideline0PointX1 = (cp.guideline0PointY - cs[1]) / ks[1];
        double guideline1PointX0 = (cp.guideline1PointY - cs[0]) / ks[0];
        double guideline1PointX1 = (cp.guideline1PointY - cs[1]) / ks[1];
        double guideline3PointX0 = (cp.guideline3PointY - cs[0]) / ks[0];
        double guideline3PointX1 = (cp.guideline3PointY - cs[1]) / ks[1];
        double guideline7PointX0 = (cp.guideline7PointY - cs[0]) / ks[0];
        double guideline7PointX1 = (cp.guideline7PointY - cs[1]) / ks[1];
        double guideline5PointX0 = (cp.guideline5PointY - cs[0]) / ks[0];
        double guideline5PointX1 = (cp.guideline5PointY - cs[1]) / ks[1];

        //是否显示5M标识，0-不显示;1-显示
        double minPointXs[] = 
        { 
                guideline0PointX0 < guideline0PointX1 ? guideline0PointX0 : guideline0PointX1,
                guideline1PointX0 < guideline1PointX1 ? guideline1PointX0 : guideline1PointX1,
                guideline3PointX0 < guideline3PointX1 ? guideline3PointX0 : guideline3PointX1,
                guideline5PointX0 < guideline5PointX1 ? guideline5PointX0 : guideline5PointX1,
                guideline7PointX0 < guideline7PointX1 ? guideline7PointX0 : guideline7PointX1
        };
        double maxPointXs[] = 
        {
                guideline0PointX0 > guideline0PointX1 ? guideline0PointX0 : guideline0PointX1,
                guideline1PointX0 > guideline1PointX1 ? guideline1PointX0 : guideline1PointX1,
                guideline3PointX0 > guideline3PointX1 ? guideline3PointX0 : guideline3PointX1,
                guideline5PointX0 > guideline5PointX1 ? guideline5PointX0 : guideline5PointX1,
                guideline7PointX0 > guideline7PointX1 ? guideline7PointX0 : guideline7PointX1
        };
        double horPointYs[] = 
        {
                cp.guideline0PointY,
                cp.guideline1PointY,
                cp.guideline3PointY,
                cp.guideline5PointY,
                cp.guideline7PointY
        };
        uint32_t colorValues[] = {dis_color_0m,  dis_color_1m,  dis_color_3m, dis_color_5m, front_lower_color};
        char* wenzi[] = {"0.5M", "1M", "3M", "5M", "7M"};
        int horsLen = 5;
        
        for (int i = 0; i < horsLen; i++)
        {
                #if ISSHOWONTHEPOSOFTHEBOOM
                if (ISHIDELINESBEYOND1M && (i == 2 || i == 3 || i == 4))
                {
                        continue;
                }
                #endif
                if (!sp.ISSHOW0MLine && i == 0)
                {
                        continue;
                }
                if (!sp.ISSHOW1MLine && i == 1)
                {
                        continue;
                }
                if (!sp.ISSHOW3MLine && i == 2)
                {
                        continue;
                }
                if (!sp.ISSHOW5MLine && i == 3)
                {
                        continue;
                }
                if (!sp.ISSHOW7MLine && i == 4)
                {
                        continue;
                }
                #if IS_ADD_3M_EXTENSION_LINE
                if (i == 2)
                {
                        maxPointXs[i] += 100;
                        minPointXs[i] -= 100;
                }
                #endif
                double xLen = maxPointXs[i] - minPointXs[i];
                for (double j = minPointXs[i]; j < maxPointXs[i]; j+=0.5)
                {
                        // if (j > minPointXs[i] + xLen * 1.0f / 7 && j < maxPointXs[i] - xLen * 1.0f / 7 && i != 0)
                        #if IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
                        
                        #if IS_ADD_3M_EXTENSION_LINE
                        if ((i != 4) && (j > minPointXs[i] + xLen * 1.0f / 3 && j < maxPointXs[i] - xLen * 1.0f / 3))
                        #else
                        if ((i != 4) && (j > minPointXs[i] + xLen * 1.0f / 7 && j < maxPointXs[i] - xLen * 1.0f / 7))
                        #endif
                        {
                                continue;
                        }
                        #else
                        
                        #if IS_ADD_3M_EXTENSION_LINE
                        if (j > minPointXs[i] + xLen * 1.0f / 3 && j < maxPointXs[i] - xLen * 1.0f / 3)
                        #else
                        if (j > minPointXs[i] + xLen * 1.0f / 7 && j < maxPointXs[i] - xLen * 1.0f / 7)
                        #endif
                        {
                                continue;
                        }
                        #endif
                        
                        // if (j > minPointXs[i] + (3 - i) * cp.hw * 7 / 2 && j < maxPointXs[i] - (3 - i) * cp.hw * 7 / 2)
                        // {
                        //         continue;
                        // }
                        double vector[] = { j / sp.offsetScale + sp.offsetX, horPointYs[i] / sp.offsetScale + sp.offsetY};
                        SetVectorPixelOS(vector, colorValues[i]);
                }
                if (cp.location == 0)
                {
                        //0m,3m,7m文字,0.5M文字要左移
                        if (i == 0)
                        {
                                LCDDisplayASCII8X16(minPointXs[i] / sp.offsetScale + sp.offsetX - 80, horPointYs[i] / sp.offsetScale + sp.offsetY - 15, colorValues[i], wenzi[i]); 
                        }
                        else
                        {
                                LCDDisplayASCII8X16(minPointXs[i] / sp.offsetScale + sp.offsetX - 50, horPointYs[i] / sp.offsetScale + sp.offsetY - 15, colorValues[i], wenzi[i]); 
                        }
                        LCDDisplayASCII8X16(maxPointXs[i] / sp.offsetScale + sp.offsetX + 20, horPointYs[i] / sp.offsetScale + sp.offsetY - 15, colorValues[i], wenzi[i]); 
                }
        }

        //轨迹线
        int lenNum = 160;
        #if ISSHOWONLY07
        double minValues[] =
        { 
                cp.zeroPointX0 < cp.zeroPointX1 ? cp.zeroPointX0 : cp.zeroPointX1,
                maxPointXs[sizeof(maxPointXs)/sizeof(maxPointXs[0]) - 1]
        };
        double maxValues[] = 
        { 
                minPointXs[sizeof(minPointXs)/sizeof(minPointXs[0]) - 1],
                cp.zeroPointX2 > cp.zeroPointX3 ? cp.zeroPointX2 : cp.zeroPointX3
        };
        #else
        double minValues[] =
        { 
                cp.zeroPointX0 < cp.zeroPointX1 ? cp.zeroPointX0 : cp.zeroPointX1,
                cp.zeroPointX2 < cp.zeroPointX3 ? cp.zeroPointX2 : cp.zeroPointX3 
        };
        double maxValues[] = 
        { 
                cp.zeroPointX0 > cp.zeroPointX1 ? cp.zeroPointX0 : cp.zeroPointX1,
                cp.zeroPointX2 > cp.zeroPointX3 ? cp.zeroPointX2 : cp.zeroPointX3
        };
        #endif

        #if ISSHOWONTHEPOSOFTHEBOOM
        if (ISHIDELINESBEYOND1M)
        {
                maxValues[0] = minPointXs[1];
                minValues[1] = maxPointXs[1];
        }
        #endif

        for (int j = 0; j < 2; j++)
        {
                double aveLen = 1.05;// (maxValues[j] - minValues[j]) / lenNum;
                for (double i = minValues[j]; i < maxValues[j]; i++)
                {
                        // 轨迹线虚线
                        // int isContinue = 0;
                        // for (int v = 0; v < lenNum / 2; v++)
                        // {
                        //         if (minValues[j] + (2 * v + 1) * aveLen <= i && i < minValues[j] + (2 * v + 2.5f) * aveLen)
                        //         {
                        //                 isContinue = 1;
                        //                 break;
                        //         }
                        // }
                        // if (isContinue)
                        // {
                        //         continue;
                        // }
                        double x = i;
                        double y = ks[j] * x + cs[j];
                        //x += offsetPointX;
                        //y += offsetPointY;
                        double vector[] = { x / sp.offsetScale + sp.offsetX, y / sp.offsetScale + sp.offsetY};
                        SetVectorPixelOS(vector, front_lower_color);
                        #if IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
                        if (i == minValues[j] && j == 1)
                        {
                                box_points[3][0] = vector[0];
                                box_points[3][1] = vector[1];
                        }
                        if (i + 1 > maxValues[j] && j == 0)
                        {
                                box_points[2][0] = vector[0];
                                box_points[2][1] = vector[1];
                        }
                        #endif
                }
        }
        
        #if !ISSHOWONLY07
        //给直行轨迹线增长
        for (int i = maxValues[0] - 8; i < maxValues[0] + 35; i++)
        {
                double x = i;
                double y = ks[0] * x + cs[0];
                //x += offsetPointX;
                //y += offsetPointY;
                double vector[] = { x / sp.offsetScale + sp.offsetX, y / sp.offsetScale + sp.offsetY};
                SetVectorPixelOS(vector, front_lower_color);
        }
        for (int i = minValues[1] - 40; i < minValues[1]; i++)
        {
                double x = i;
                double y = ks[1] * x + cs[1];
                //x += offsetPointX;
                //y += offsetPointY;
                double vector[] = { x / sp.offsetScale + sp.offsetX, y / sp.offsetScale + sp.offsetY};
                SetVectorPixelOS(vector, front_lower_color);
        }
        #endif

        #if IS_DRAW_TRAJECTORY_ON_THE_WHEEL
        double zValues[] = { -baseParams.c1 / 2 - baseParams.d1 / 2, baseParams.c1 / 2 + baseParams.d1 / 2 };
        int maxJ = sizeof(zValues)/sizeof(zValues[0]);
        #if ISSHOWONTHEPOSOFTHEBOOM
        if (ISHIDELINESBEYOND1M)
        {
                maxJ = 0;
        }
        #endif
        for (int j=0;j<maxJ;j++)
        {
                for (double i = -cameraParams0.guideline7; i <= 0; i += 0.2)
                {
                        double vecValue[] = {i, tireSize, zValues[j], 1};
                        double result[] = {0, 0, 0, 0};
                        double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams0, screenParams0, 1);
                        if (IsScreenVector(vecScreen, screenParams0))
                        {
                                SetVectorPixelOS(vecScreen, front_upper_color);
                                #if IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
                                if (i == -cameraParams0.guideline7)
                                {
                                        box_points[j][0] = vecScreen[0];
                                        box_points[j][1] = vecScreen[1];
                                }
                                #endif
                        }
                }
        }
        #if IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
        DrawThreeDimensionalAuxiliaryBox();
        #endif
        #endif
}

void DrawTrajectory(double angle, int isMirror, double limitPos[])
{

        //如果角度为0则单独绘制0度的避障轨迹参考线
        if  (angle == 0)
        {
                TrajectoryZeroDegreeLine(cameraParams0, screenParams0);
                // //在坐标系Y=0构建好两条直线，再进行透视
                // //两条直线Y值固定，X从-guideline7到w
                // double zValues[] = { -c1 / 2 - baseParams.d1 / 2, f / 2 };
                // for (int j=0;j<sizeof(zValues)/sizeof(zValues[0]);j++)
                // {
                //         int lenNum = 140;
                //         float aveLen = 2;
                //         for (double i = -cameraParams0.guideline7; i <= -cameraParams0.guideline0+1; i+=0.2)
                //         {
                //                 int isContinue = 0;
                //                 for (int v = 0; v < lenNum / 2; v++)
                //                 {
                //                         if (-cameraParams0.mw - 5 + (2 * v + 1) * aveLen <= i && i < -cameraParams0.mw - 5 + (2 * v + 2.5) * aveLen)
                //                         {
                //                                 isContinue = 1;
                //                                 break;
                //                         }
                //                 }

                //                 if (isContinue)
                //                 {
                //                         continue;
                //                 }

                //                 double vecValue[] = {i, 0, zValues[j], 1};
                //                 double result[] = {0, 0, 0, 0};
                                // double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams0, screenParams0);
                                // if (IsScreenVector(vecScreen, screenParams0))
                //                 {
                //                         SetVectorPixelOS(vecScreen, GREEN);
                //                 }
                //         }
                // }
        }
        else
        {
                double vecValue00[] = {-cameraParams0.mw, 0, (-baseParams.c1/2-baseParams.d1/2), 1}; 

                double vecValue11[] = {-cameraParams0.mw, 0, (baseParams.f/2), 1}; 

                double result0[] = {0, 0, 0, 0};
                double *pos00 = MatrixTransformHorizontalLine(vecValue00, result0); 
                double result1[] = {0, 0, 0, 0};
                double *pos11 = MatrixTransformHorizontalLine(vecValue11, result1); 

                // //求最上面和最下面的两个点的直线方程,其中斜率需要修正
                double k = (pos00[2] - pos11[2]) / (pos00[0] - pos11[0]);
                double c = pos11[2] - pos11[0] * k;

                //求直线分别与铲外侧轨迹方程、前轮内侧轨迹方程的交点,经测试如果大于30度则与前轮内侧可能没有交点,这时使用铲内侧轨迹方程计算
                double result2[] = {0, 0, 0, 0};
                double *pos4 = GetPointLineAndCircle(k, c, baseParams.g + baseParams.b2, -GetR2(angle), GetR1(angle) - baseParams.c1 / 2 - baseParams.d1 / 2, result2);     //测试去掉此方法镜像
                double result3[] = {0, 0, 0, 0};
                double *pos5 = GetPointLineAndCircle(k, c, baseParams.g + baseParams.b2 , -GetR2(angle), sqrt(pow((GetR1(angle) + baseParams.f / 2), 2) + pow(baseParams.e, 2)), result3);     //测试去掉此方法镜像

                //判断如果内侧轨迹线和横线没有交点,则进行横线向下延长补偿处理
                if (isnan(pos4[0]) || isnan(pos4[1]) || isnan(pos4[2]) || isnan(pos4[3]))
                {
                        pos4[0] = pos5[0] + 20;
                        pos4[1] = 0;
                        pos4[2] = k * pos4[0] + c;
                        pos4[3] = 1;
                }
				
                double r1 = GetR1(angle);
                double r2 = GetR2(angle);
                double maxR = r1 > r2 ? r1 : r2;
                maxR = maxR > num ? num : maxR;
                //for (int i = -num / 2; i < num / 2; i++)
                int index = 0;
                // printf("%f~%f\n", baseParams.g - maxR, baseParams.g + maxR);
                // double len = 0;
                double len = 0.2;//2 * maxR / num;
                double qianChanValueTemp0, qianlunneiceValueTemp0, qianChanValueTemp1, qianlunneiceValueTemp1;
				
                int lenNum = 140;
                double aveLen = 2 * maxR / lenNum;
				
                for (double i = baseParams.g - maxR; i < 0; i+=len, index+=0.2)
                // for (double i = baseParams.g - maxR; i < baseParams.g + maxR; i+=len, index++)
                {
			int isContinue = 0;
                        for (int v = 0; v < lenNum / 2; v++)
                        {
                                if (-maxR + (2 * v + 1) * aveLen <= i && i < -maxR + (2 * v + 2.5) * aveLen)
                                {
                                        isContinue = 1;
                                        break;
                                }
                        }
                        // double lenTemp = 1 - (fabs(i - baseParams.g) / maxR);
                        // len = (0.2 > lenTemp ? 0.2 : lenTemp);
                        // printf("len = %lf\n", len);

                        // len = (index++ + 1) / (2.0 * maxR);
                        // int i=-14;
                        #if ISSHOWONLY07
                        if ((i > limitPos[0] && i < limitPos[1] && fabs(angle) <= 25) || fabs(angle) > 25)
                        #endif
                        {
                                #if ISSHOWONTHEPOSOFTHEBOOM
                                if (i > limitPos[0] && i < limitPos[1])
                                #endif
                                {
                                        double qianChanValue = GetQianchanY(angle, i, isMirror, 1);
                                        //判断轨迹点是否在圆内或者圆上，如果在圆外则会返回nan
                                        if (!isnan(qianChanValue) && (i < pos5[0] || !isContinue))
                                        {
                                                double vecValue[] = {i, 0, qianChanValue, 1};
                                                double result[] = {0, 0, 0, 0};
                                                double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams0, screenParams0, 0);
                                                if (IsScreenVector(vecScreen, screenParams0))
                                                {
                                                        // printf("vecScreen x: %lf\n", vecScreen[0]);
                                                        SetVectorPixelOS(vecScreen, front_lower_color);
                                                }
                                        }
                                }
                        }
                        #if IS_DRAW_TRAJECTORY_ON_THE_WHEEL
                        #if ISSHOWONLY07
                        if ((i > limitPos[0] && fabs(angle) <= 25) || fabs(angle) > 25)
                        #endif
                        {
                                #if ISSHOWONTHEPOSOFTHEBOOM
                                if (!ISHIDELINESBEYOND1M)
                                #endif
                                {
                                        double qianChanValue = GetQianchanY(angle, i, isMirror, 1);
                                        //判断轨迹点是否在圆内或者圆上，如果在圆外则会返回nan
                                        if (!isnan(qianChanValue) && (i < pos5[0] || !isContinue))
                                        {
                                                double vecValue[] = {i, tireSize, qianChanValue, 1};
                                                double result[] = {0, 0, 0, 0};
                                                double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams0, screenParams0, 0);
                                                if (IsScreenVector(vecScreen, screenParams0))
                                                {
                                                        // printf("vecScreen x: %lf\n", vecScreen[0]);
                                                        SetVectorPixelOS(vecScreen, front_upper_color);

                                                        // #if IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
                                                        // if (box_points[1][0] == 0 && box_points[1][1] == 0 &&
                                                        //         !isnan(vecScreen[0]) && !isnan(vecScreen[1]))     //7M
                                                        // {
                                                        //         box_points[1][0] = vecScreen[0];
                                                        //         box_points[1][1] = vecScreen[1];
                                                        // }
                                                        // #endif
                                                }
                                        }
                                }
                        }
                        #endif
                        
                        #if ISSHOWONLY07
                        if ((i > limitPos[2] && i < limitPos[3] && fabs(angle) <= 25) || fabs(angle) > 25)
                        #endif
                        {
                                #if ISSHOWONTHEPOSOFTHEBOOM
                                if (i > limitPos[2] && i < limitPos[3])
                                #endif
                                {
                                        double qianlunneiceValue = GetQianlunneiceY(angle, i, isMirror, 1);
                                        if (!isnan(qianlunneiceValue) &&
                                                (angle < cameraParams0.showAngle && (i < pos4[0] || !isContinue)) || 
                                                (angle >= cameraParams0.showAngle))
                                        {
                                                double vecValue[] = {i, 0, qianlunneiceValue, 1};
                                                double result[] = {0, 0, 0, 0};
                                                double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams0, screenParams0, 0);
                                                if (IsScreenVector(vecScreen, screenParams0))
                                                {
                                                        // printf("vecScreen x: %lf\n", vecScreen[0]);
                                                        SetVectorPixelOS(vecScreen, front_lower_color);
                                                }
                                        }
                                }
                        }
                        #if IS_DRAW_TRAJECTORY_ON_THE_WHEEL
                        #if ISSHOWONLY07
                        if ((i > limitPos[2] && fabs(angle) <= 25) || fabs(angle) > 25)
                        #endif
                        {
                                #if ISSHOWONTHEPOSOFTHEBOOM
                                if (!ISHIDELINESBEYOND1M)
                                #endif
                                {
                                        double qianlunneiceValue = GetQianlunneiceY(angle, i, isMirror, 1);
                                        if (!isnan(qianlunneiceValue) &&
                                                (angle < cameraParams0.showAngle && (i < pos4[0] || !isContinue)) || 
                                                (angle >= cameraParams0.showAngle))
                                        {
                                                double vecValue[] = {i, tireSize, qianlunneiceValue, 1};
                                                double result[] = {0, 0, 0, 0};
                                                double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams0, screenParams0, 0);
                                                if (IsScreenVector(vecScreen, screenParams0))
                                                {
                                                        // printf("vecScreen x: %lf\n", vecScreen[0]);
                                                        SetVectorPixelOS(vecScreen, front_upper_color);

                                                        // #if IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
                                                        // if (box_points[0][0] == 0 && box_points[0][1] == 0 &&
                                                        //         !isnan(vecScreen[0]) && !isnan(vecScreen[1]))     //7M
                                                        // {
                                                        //         box_points[0][0] = vecScreen[0];
                                                        //         box_points[0][1] = vecScreen[1];
                                                        // }
                                                        // #endif
                                                }
                                        }
                                }
                        }
                        #endif

                        //补充轨迹线,相当于画出另外半个圆,根据圆的求根公式,取Δ分别为正负
                        #if 0
                        //角度大于25度才显示补充轨迹线
                        if (angle <25 || i > baseParams.g)
                        {
                                continue;
                        }
                        qianChanValue = GetQianchanY(angle, i, isMirror, -1);
                        qianlunneiceValue = GetQianlunneiceY(angle, i, isMirror, -1);
						
                        //判断轨迹点是否在圆内或者圆上，如果在圆外则会返回nan
                        if (!isnan(qianChanValue))
                        {
                                double vecValue[] = {i, 0, qianChanValue, 1};
                                double result[] = {0, 0, 0, 0};
                                double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams0, screenParams0);
                                if (IsScreenVector(vecScreen, screenParams0))
                                {
                                        // printf("vecScreen x: %lf\n", vecScreen->data[0]);
                                        SetVectorPixelOS(vecScreen, front_lower_color);
                                }
                        }
                        if (!isnan(qianlunneiceValue))
                        {
                                double vecValue[] = {i, 0, qianlunneiceValue, 1};
                                double result[] = {0, 0, 0, 0};
                                double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams0, screenParams0);
                                if (IsScreenVector(vecScreen, screenParams0))
                                {
                                        // printf("vecScreen x: %lf\n", vecScreen->data[0]);
                                        SetVectorPixelOS(vecScreen, front_lower_color);
                                }
                        }
                        #endif
                }
        }
}

void DrawHorizontalLine(double angle, int isMirror, double limitPos[])
{
        // int xValues[] = {-baseParams.e-ee-(baseParams.b1-baseParams.g),  -baseParams.e-ee-(baseParams.b1-baseParams.g)+cameraParams0.guideline3, -cameraParams0.guideline0};

        //是否显示5M标识，0-不显示;1-显示
        double xValues[] = { -cameraParams0.guideline7, -cameraParams0.guideline5, -cameraParams0.guideline3, -cameraParams0.guideline1, -cameraParams0.guideline0 };
        uint32_t colorValues[] = {front_lower_color,  dis_color_5m, dis_color_3m, dis_color_1m, dis_color_0m};
        char* wenzi[] = {"7M", "5M", "3M", "1M", "0.5M"};

        if  (angle == 0)
        {
                // for (int j=0;j<sizeof(xValues)/sizeof(xValues[0]);j++)
                // {
                //         for (double i=-baseParams.c1/2-baseParams.d1/2;i<-baseParams.c1/2-baseParams.d1/2+cameraParams0.hw;i++)
                //         {
                //                 double vecValue[] = {xValues[j], 0, isMirror * i, 1};
                //                 double result[] = {0, 0, 0, 0};
                //                 double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams0, screenParams0);
                //                 if (IsScreenVector(vecScreen, screenParams0))
                //                 {
                //                         SetVectorPixelOS(vecScreen, colorValues[j]);
                //                         if (i == -baseParams.c1/2-baseParams.d1/2)
                //                         {
                //                                 LCDDisplayASCII8X16(vecScreen[0] - 40, vecScreen[1] - 15, colorValues[j], wenzi[j]);
                //                         }
                //                 }
                //         }
                //         for (double i=baseParams.f/2;i>baseParams.f/2-cameraParams0.hw;i--)
                //         {
                //                 double vecValue[] = {xValues[j], 0, isMirror * i, 1};
                //                 double result[] = {0, 0, 0, 0};
                //                 double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams0, screenParams0);
                //                 if (IsScreenVector(vecScreen, screenParams0))
                //                 {
                //                         SetVectorPixelOS(vecScreen, colorValues[j]);
                //                         if (i == baseParams.f/2)
                //                         {
                //                                 LCDDisplayASCII8X16(vecScreen[0] + 20, vecScreen[1] - 15, colorValues[j], wenzi[j]);
                //                         }
                //                 }
                //         }
                // }
        }
        else
        {
                for (int j=0;j<sizeof(xValues)/sizeof(xValues[0]);j++)
                {
                        #if ISSHOWONTHEPOSOFTHEBOOM
                        if (ISHIDELINESBEYOND1M && (j == 0 || j == 1 || j == 2))
                        {
                                continue;
                        }
                        #endif
                        if (!screenParams0.ISSHOW0MLine && j == 4)
                        {
                                continue;
                        }
                        if (!screenParams0.ISSHOW1MLine && j == 3)
                        {
                                continue;
                        }
                        if (!screenParams0.ISSHOW3MLine && j == 2)
                        {
                                continue;
                        }
                        if (!screenParams0.ISSHOW5MLine && j == 1)
                        {
                                continue;
                        }
                        if (!screenParams0.ISSHOW7MLine && j == 0)
                        {
                                continue;
                        }
                        {       
                                // printf("baseParams.f: [%lf]\nc1: [%lf]\n", baseParams.f, baseParams.c1);
                                //每条横线中间隔开,所以又四个点,求出4个点在旋转Y度后的世界坐标

                                // double vecValue0[] = {xValues[j], 0, isMirror * (-baseParams.c1/2-baseParams.d1/2), 1};
                                double vecValue0[] = {xValues[j], 0, (-baseParams.c1/2-baseParams.d1/2), 1};       //测试去掉此方法镜像

                                // double vecValue1[] = {xValues[j], 0, isMirror * (baseParams.f/2), 1};
                                double vecValue1[] = {xValues[j], 0, (baseParams.f/2), 1};       //测试去掉此方法镜像

                                double result0[] = {0, 0, 0, 0};
                                double *pos0;      //测试去掉此方法镜像
                                
                                //是否按照实际7M线画横线,否则按照车辆转弯圆心与外圆7M处连线画交点
                                #if IS_DRAW_ACTUAL_7M_HOR_LINE
                                pos0 = MatrixTransformHorizontalLine(vecValue0, result0); 
                                #else
                                double roundDot[] = {baseParams.g + baseParams.b2, 0, -GetR2(angle) * isMirror, 0};
                                pos0 = roundDot;
                                #endif

                                double result1[] = {0, 0, 0, 0};
                                double *pos1 = MatrixTransformHorizontalLine(vecValue1, result1);       //测试去掉此方法镜像

                                // // double vecValue2[] = {xValues[j], 0, isMirror * (-baseParams.c1/2-baseParams.d1/2+cameraParams0.hw), 1};
                                // double vecValue2[] = {xValues[j], 0, (-baseParams.c1/2-baseParams.d1/2+cameraParams0.hw), 1};      //测试去掉此方法镜像

                                // // double vecValue3[] = {xValues[j], 0, isMirror * (baseParams.f/2-cameraParams0.hw), 1};
                                // double vecValue3[] = {xValues[j], 0, (baseParams.f/2-cameraParams0.hw), 1};      //测试去掉此方法镜像

                                // double result2[] = {0, 0, 0, 0};
                                // double *pos2 = MatrixTransformHorizontalLine(vecValue2, result2);      //测试去掉此方法镜像
                                // double result3[] = {0, 0, 0, 0};
                                // double *pos3 = MatrixTransformHorizontalLine(vecValue3, result3);      //测试去掉此方法镜像


                                //求最上面和最下面的两个点的直线方程,其中斜率需要修正
                                double k = (pos0[2] - pos1[2]) / (pos0[0] - pos1[0]);
                                double c = pos1[2] - pos1[0] * k;

                                // printf("k: [%lf]\nc: [%lf]\n", k, c);

                                //求直线分别与铲外侧轨迹方程、前轮内侧轨迹方程的交点,经测试如果大于22度则与前轮内侧可能没有交点,这时使用铲内侧轨迹方程计算
                                double result4[] = {0, 0, 0, 0};
                                double *pos4 = GetPointLineAndCircle(k, c, baseParams.g + baseParams.b2, -GetR2(angle) * isMirror, GetR1(angle) - baseParams.c1 / 2 - baseParams.d1 / 2, result4);
                                // Vector *pos4 = GetPointLineAndCircle(k, c, baseParams.g + baseParams.b2, -GetR2(angle), GetR1(angle) - baseParams.c1 / 2 - baseParams.d1 / 2);     //测试去掉此方法镜像
                                // if ((pos4[0] != pos4[0]) ||  (pos4[1] != pos4[1]) ||  (pos4[2] != pos4[2]) ||  (pos4[3] != pos4[3]))
                                
                                double result5[] = {0, 0, 0, 0};
                                double *pos5 = GetPointLineAndCircle(k, c, baseParams.g + baseParams.b2 , -GetR2(angle) * isMirror, sqrt(pow((GetR1(angle) + baseParams.f / 2), 2) + pow(baseParams.e, 2)), result5);
                                // Vector *pos5 = GetPointLineAndCircle(k, c, baseParams.g + baseParams.b2, -GetR2(angle), sqrt(pow((GetR1(angle) + baseParams.f / 2), 2) + pow(baseParams.e, 2)));     //测试去掉此方法镜像

                                //是否按照实际7M线画横线,否则按照车辆转弯圆心与外圆7M处连线画交点
                                #if IS_DRAW_ACTUAL_7M_HOR_LINE
                                //判断如果内侧轨迹线和横线没有交点,则进行横线向下延长补偿处理
                                if (isnan(pos4[0]) || isnan(pos4[1]) || isnan(pos4[2]) || isnan(pos4[3]))
                                {
                                        // pos4 = GetPointLineAndCircle(k, c, baseParams.g + GetB2(angle), -GetR2(angle) * isMirror, sqrt(pow(GetR1(angle), 2) + pow(baseParams.e, 2)));
                                        pos4[0] = pos5[0] + 20;
                                        pos4[1] = 0;
                                        pos4[2] = k * pos4[0] + c;
                                        pos4[3] = 1;
                                }
                                #endif

                                // printf("pos4: [%lf,%lf,%lf]\n", pos4[0], pos4[1], pos4[2]);
                                // printf("pos5: [%lf,%lf,%lf]\n", pos5[0], pos5[1], pos5[2]);

				//计算中间缺失的距离以及点
                                double *minPos = pos5[0] < pos4[0] ? pos5 : pos4;
                                double *maxPos = pos5[0] > pos4[0] ? pos5 : pos4;
                                double dis = Distance(minPos, maxPos);
                                //float mDis = (dis - cameraParams0.hw) / 2;
                                double resultM[] = {0, 0, 0, 0};
                                double *mPos = GetCenter(maxPos, minPos, resultM);
                                //Debug.Log(string.Format("minPos: [{0}, {1}], maxPos: [{2}, {3}], mPos: [{4}, {5}]", 
                                //    minPos.x, minPos.z, maxPos.x, maxPos.z, mPos.x, mPos.z));
                                double r = dis * 1 / 3;//cameraParams0.hw / 2;
                                double result6[] = {0, 0, 0, 0};
                                double *pos6 = GetPointLineAndCircle(k, c, mPos[0], mPos[2], r, result6);
                                double result7[] = {0, 0, 0, 0};
                                double *pos7 = GetPointLineAndCircle1(k, c, mPos[0], mPos[2], r, result7);
                                double *minPos1 = pos6[0] < pos7[0] ? pos6 : pos7;
                                double *maxPos1 = pos6[0] > pos7[0] ? pos6 : pos7;

                                #if ISSHOWONLY07
                                if (j == 0 && !ISHIDELINESBEYOND1M)
                                {
                                        #if ISSHOWONTHEPOSOFTHEBOOM
                                        limitPos[0] = minPos[0];
                                        limitPos[2] = maxPos[0];
                                        #endif
                                }
                                else if (j == 3 && ISHIDELINESBEYOND1M)
                                {
                                        #if ISSHOWONTHEPOSOFTHEBOOM
                                        limitPos[0] = minPos[0];
                                        limitPos[2] = maxPos[0];
                                        #endif
                                }
                                else if (j == sizeof(xValues)/sizeof(xValues[0]) - 1)
                                {
                                        limitPos[1] = minPos[0];
                                        limitPos[3] = maxPos[0];
                                }
                                #endif

                                //使用与轨迹求出的交点画线,并在中间断开一截
                                double len = fabs(pos4[0] - pos5[0]);
                                // for (double i=(pos4[0] < pos5[0] ? pos4[0] : pos5[0]);i<=(pos4[0] > pos5[0] ? pos4[0] : pos5[0]);i+=len/50)
                                #if IS_ADD_3M_EXTENSION_LINE
                                if (j == 2)
                                {
                                        minPos[0] -= (fabs(angle) / 3);
                                        maxPos[0] += (fabs(angle) / 3);
                                }
                                #endif
                                for (double i = minPos[0]; i<= maxPos[0]; i+=len/120)
                                {
                                        #if IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
                                        if ((j != 0) && (i < maxPos1[0] && i > minPos1[0] && j != sizeof(xValues)/sizeof(xValues[0]) - 1))
                                        {
                                                continue;
                                        }
                                        #else
                                        if (i < maxPos1[0] && i > minPos1[0] && j != sizeof(xValues)/sizeof(xValues[0]) - 1)
                                        {
                                                continue;
                                        }
                                        #endif
                                        
                                        double vecValue[] = {i, 0, k * i + c, 1};
                                        double result[] = {0, 0, 0, 0};
                                        double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams0, screenParams0, 1);
                                        if (IsScreenVector(vecScreen, screenParams0))
                                        {
                                                SetVectorPixelOS(vecScreen, colorValues[j]);

                                                #if IS_SCHEMATIC_DIAGRAM_OF_AUXILIARY_COORDINATE_SYS
                                                if (j == 0)     //7M
                                                {
                                                        if (i == minPos[0])
                                                        {
                                                                leftTopPoint[0] = vecScreen[0];
                                                                leftTopPoint[1] = vecScreen[1];
                                                        }
                                                        else if (i + len/120 > maxPos[0])
                                                        {
                                                                rightTopPoint[0] = vecScreen[0];
                                                                rightTopPoint[1] = vecScreen[1];
                                                        }
                                                }
                                                else if (j == sizeof(xValues)/sizeof(xValues[0]) - 1)           //0M
                                                {
                                                        if (i == minPos[0])
                                                        {
                                                                leftBottomPoint[0] = vecScreen[0];
                                                                leftBottomPoint[1] = vecScreen[1];
                                                        }
                                                        else if (i + len/120 > maxPos[0])
                                                        {
                                                                rightBottomPoint[0] = vecScreen[0];
                                                                rightBottomPoint[1] = vecScreen[1];
                                                        }
                                                }
                                                #endif

                                                #if IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
                                                if (j == 0)     //7M
                                                {
                                                        if (i == minPos[0])
                                                        {
                                                                box_points[3][0] = vecScreen[0];
                                                                box_points[3][1] = vecScreen[1];

                                                                double vecValue3[] = {i, tireSize, k * i + c, 1};
                                                                double result3[] = {0, 0, 0, 0};
                                                                double *vecScreen3 = MatrixTransformWToS(vecValue3, result3, cameraParams0, screenParams0, 1);
                                                                box_points[1][0] = vecScreen3[0];
                                                                box_points[1][1] = vecScreen3[1];
                                                        }
                                                        else if (i + len/120 > maxPos[0])
                                                        {
                                                                box_points[2][0] = vecScreen[0];
                                                                box_points[2][1] = vecScreen[1];
                                                                
                                                                double vecValue3[] = {i, tireSize, k * i + c, 1};
                                                                double result3[] = {0, 0, 0, 0};
                                                                double *vecScreen3 = MatrixTransformWToS(vecValue3, result3, cameraParams0, screenParams0, 1);
                                                                box_points[0][0] = vecScreen3[0];
                                                                box_points[0][1] = vecScreen3[1];
                                                        }
                                                }
                                                #endif

                                                double fontX, fontY;
                                                if (i == minPos[0])
                                                {
                                                        if (isMirror == 1)
                                                        {
                                                                fontX = vecScreen[0] + 20;
                                                                fontY = vecScreen[1] - 15;
                                                                // LCDDisplayASCII8X16(vecScreen[0] + 20, vecScreen[1] - 15, colorValues[j], wenzi[j]); 
                                                        }
                                                        else
                                                        {
                                                                //0.5M文字要左移
                                                                if(j == sizeof(xValues)/sizeof(xValues[0]) - 1)
                                                                { 
                                                                        fontX = vecScreen[0] - 80;
                                                                }
                                                                else
                                                                {
                                                                        fontX = vecScreen[0] - 50;
                                                                }
                                                                fontY = vecScreen[1] - 15;
                                                                // LCDDisplayASCII8X16(vecScreen[0] - 40, vecScreen[1] - 15, colorValues[j], wenzi[j]); 
                                                        }
                                                }
                                                else if (i + len/120 > maxPos[0])
                                                {   
                                                        if (isMirror == 1)
                                                        {
                                                                //0.5M文字要左移
                                                                if(j == sizeof(xValues)/sizeof(xValues[0]) - 1)
                                                                {
                                                                        fontX = vecScreen[0] - 80;
                                                                }
                                                                else
                                                                {
                                                                        fontX = vecScreen[0] - 50;
                                                                }
                                                                fontY = vecScreen[1] - 15;
                                                                // LCDDisplayASCII8X16(vecScreen[0] - 40, vecScreen[1] - 15, colorValues[j], wenzi[j]);
                                                        }
                                                        else
                                                        {
                                                                fontX = vecScreen[0] + 20;
                                                                fontY = vecScreen[1] - 15;
                                                                // LCDDisplayASCII8X16(vecScreen[0] + 20, vecScreen[1] - 15, colorValues[j], wenzi[j]);
                                                        }
                                                }
                                                if ((i == minPos[0] || i + len/120 > maxPos[0]) && 
                                                (fontX <= screenParams0.viewMaxX - 24 && fontY <= screenParams0.viewMaxY - 20 && fontX >= screenParams0.viewMinX + 20 && fontY >= screenParams0.viewMinY + 20))
                                                {
                                                        LCDDisplayASCII8X16(fontX, fontY, colorValues[j], wenzi[j]); 
                                                }
                                        }
                                }
                        }
                }
        }
}

#if IS_SCHEMATIC_DIAGRAM_OF_AUXILIARY_COORDINATE_SYS
//初始化辅助坐标系示意点
void InitSchematicDiagramOfAuxiliaryCoordinatePoint()
{
        leftTopPoint[0] = 0;
        leftTopPoint[1] = 0;
        rightTopPoint[0] = 0;
        rightTopPoint[1] = 0;
        leftBottomPoint[0] = 0;
        leftBottomPoint[1] = 0;
        rightBottomPoint[0] = 0;
        rightBottomPoint[1] = 0;
}
#endif

//绘制辅助坐标系示意
void DrawSchematicDiagramOfAuxiliaryCoordinateSystem()
{
        #if IS_SCHEMATIC_DIAGRAM_OF_AUXILIARY_COORDINATE_SYS
        int len = axisHeight;
        double pointX = leftTopPoint[1];
        for (int j = pointX; j > pointX - len; j --)
        {
                leftTopPoint[1] = j;
                SetVectorPixelOS(leftTopPoint, front_upper_color);
        }
        pointX = rightTopPoint[1];
        for (int j = pointX; j > pointX - len; j --)
        {
                rightTopPoint[1] = j;
                SetVectorPixelOS(rightTopPoint, front_upper_color);
        }
        pointX = leftBottomPoint[1];
        for (int j = pointX; j > pointX - len; j --)
        {
                leftBottomPoint[1] = j;
                SetVectorPixelOS(leftBottomPoint, front_upper_color);
        }
        pointX = rightBottomPoint[1];
        for (int j = pointX; j > pointX - len; j --)
        {
                rightBottomPoint[1] = j;
                SetVectorPixelOS(rightBottomPoint, front_upper_color);
        }
        InitSchematicDiagramOfAuxiliaryCoordinatePoint();
        #endif
}

#if IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
//初始立体辅助框坐标
void InitThreeDimensionalAuxiliaryBoxPoint()
{
        box_points[0][0] = 0;
        box_points[0][1] = 0;
        box_points[1][0] = 0;
        box_points[1][1] = 0;
        box_points[2][0] = 0;
        box_points[2][1] = 0;
        box_points[3][0] = 0;
        box_points[3][1] = 0;
}
#endif

//根据轨迹线末端画立体辅助框,同时需要打开上一项(是否画车轮上轨迹线)
void DrawThreeDimensionalAuxiliaryBox()
{
        #if ISSHOWONTHEPOSOFTHEBOOM
        if (ISHIDELINESBEYOND1M)
        {
                return;
        }
        #endif
        #if IS_DRAW_THREE_DIMENSIONAL_AUXILIARY_BOX
        // printf("(%lf, %lf)     (%lf, %lf)    (%lf, %lf)    (%lf, %lf)\n", 
        //         box_points[0][0], box_points[0][1], box_points[1][0], box_points[1][1], box_points[2][0], box_points[2][1], box_points[3][0], box_points[3][1]);
        if (box_points[0][0] != 0 && box_points[0][1] != 0 && box_points[1][0] != 0 && box_points[1][1] != 0 && 
                box_points[2][0] != 0 && box_points[2][1] != 0 && box_points[3][0] != 0 && box_points[3][1] != 0 && 
                IsScreenVector(box_points[0], screenParams0) && IsScreenVector(box_points[1], screenParams0) && 
                IsScreenVector(box_points[2], screenParams0) && IsScreenVector(box_points[3], screenParams0))
        {
                DrawStraightLine(box_points[0], box_points[1], front_upper_color, screenParams0);
                DrawStraightLine(box_points[1], box_points[3], front_lower_color, screenParams0);
                DrawStraightLine(box_points[2], box_points[0], front_lower_color, screenParams0);
        }
        InitThreeDimensionalAuxiliaryBoxPoint();
        #endif
}

void Trajectory(double angle)
{
        //如果角度值小于阈值,则认为是0度
        #if 1
        if (fabs(angle) < cameraParams0.angleThreshold)
        {
                angle = 0;
        }
        #endif
        //因为需要用到isMirror,放到了DrawTrajectory函数中
        // InitParamMatrix(angle);
        int isMirror = 1;
        if (angle < 0)
        {
                isMirror = -1;
                angle =fabs(angle);
        }
        //isMirror左正右负
        InitParamMatrix(angle, isMirror);
        double limitPos[] = {0, 0, 0, 0};
        DrawHorizontalLine(angle, isMirror, limitPos);
        if (angle != 0)
        {
                DrawThreeDimensionalAuxiliaryBox();
        }
        DrawTrajectory(angle, isMirror, limitPos);
        DrawSchematicDiagramOfAuxiliaryCoordinateSystem();
        FreeParamMatrix();
}

void DrawTrajectoryBack(double angle, int isMirror)
{
        if (angle == 0)
        {
                TrajectoryZeroDegreeLine(cameraParams1, screenParams1);
        }
        else
        {
                double maxLen = 130; //大概130-45=85分米
                double r_houz = GetR2(angle);
                double rs[] = { r_houz - baseParams.c2 / 2 - baseParams.d2 / 2, r_houz + baseParams.c2 / 2 + baseParams.d2 / 2 };
                for (int i = 0; i < 2; i++)
                {
                        double len = baseParams.g + baseParams.b2 + rs[i] > maxLen ? maxLen : baseParams.g + baseParams.b2 + rs[i];
                        for (double j = baseParams.g + baseParams.b2; j < len; j+=0.2)
                        {
                                if (i == 0)
                                {
                                        double houlunneiceValue = GetHoulunneiceY(angle, j, isMirror, 1);
                                        double vecValue[] = {j, 0, houlunneiceValue, 1};
                                        double result[] = {0, 0, 0, 0};
                                        double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams1, screenParams1, 0);
                                        // printf("vecScreen[0],vecScreen[1] : %lf , %lf\n", vecScreen[0],vecScreen[1]);
                                        if (IsScreenVector(vecScreen, screenParams1))
                                        {
                                                SetVectorPixelOS(vecScreen, front_lower_color);
                                        }
                                }
                                else
                                {
                                        double houlunwaiceValue = GetHoulunwaiceY(angle, j, isMirror, 1);
                                        double vecValue[] = {j, 0, houlunwaiceValue, 1};
                                        double result[] = {0, 0, 0, 0};
                                        double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams1, screenParams1, 0);
                                        if (IsScreenVector(vecScreen, screenParams1))
                                        {
                                                SetVectorPixelOS(vecScreen, front_lower_color);
                                        }
                                }
                        }
                }
        }
}

// 画后倒车距离参考线
void DrawHorizontalBackLine(double angle, int isMirror)
{
        double xValues[] = { cameraParams1.guideline7, cameraParams1.guideline5, cameraParams1.guideline3, cameraParams1.guideline1, cameraParams1.guideline0 };
        uint32_t colorValues[] = {front_lower_color,  dis_color_5m, dis_color_3m, dis_color_1m, dis_color_0m};
        char* wenzi[] = {"7M", "5M", "3M", "1M", "0.5M"};
        double rw2c = baseParams.rw2c;
        double rf2c = baseParams.rf2c;
        if (angle != 0)
        {
                int len = sizeof(xValues)/sizeof(xValues[0]);
                for (int i = 0; i < len; i++)
                {
                        if (!screenParams1.ISSHOW7MLine && i == 0)
                        {
                                continue;
                        }
                        if (!screenParams1.ISSHOW5MLine && i == 1)
                        {
                                continue;
                        }
                        if (!screenParams1.ISSHOW3MLine && i == 2)
                        {
                                continue;
                        }
                        if (!screenParams1.ISSHOW1MLine && i == 3)
                        {
                                continue;
                        }
                        if (!screenParams1.ISSHOW0MLine && i == 4)
                        {
                                continue;
                        }
                        double fixedX = rw2c + xValues[i];
                        double ys[] = {0, 0, 0, 0};
                        double *yss = GetFixedXTwoCircleSolutions(fixedX, baseParams.g + baseParams.b2, - isMirror * GetR2(angle), GetR2(angle) - baseParams.c2 / 2 - baseParams.d2 / 2, GetR2(angle) + baseParams.c2 / 2 + baseParams.d2 / 2, isMirror, ys);
                        double minYs = yss[0] < yss[1] ? yss[0] : yss[1];
                        double maxYs = yss[0] > yss[1] ? yss[0] : yss[1];
                        #if IS_ADD_3M_EXTENSION_LINE
                        if (i == 2) 
                        {
                                minYs -= 10;
                                maxYs += 10;
                        }
                        #endif
                        double xLen = maxYs - minYs;
                        for (double j = minYs; j < maxYs; j += 0.5)
                        {
                                // if (j > minYs + xLen * 1.0f / 7 && j < maxYs - xLen * 1.0f / 7 && i != 0)
                                #if IS_ADD_3M_EXTENSION_LINE
                                if (j > minYs + xLen * 1.0f / 3 && j < maxYs - xLen * 1.0f / 3)
                                #else
                                if (j > minYs + xLen * 1.0f / 7 && j < maxYs - xLen * 1.0f / 7)
                                #endif
                                {
                                        continue;
                                }
                                double vecValue[] = {fixedX, 0, j, 1};
                                double result[] = {0, 0, 0, 0};
                                double *vecScreen = MatrixTransformWToS(vecValue, result, cameraParams1, screenParams1, 1);
                                // printf("vecScreen[0],vecScreen[1] : %lf , %lf\n", vecScreen[0],vecScreen[1]);
                                if (IsScreenVector(vecScreen, screenParams1))
                                {
                                        SetVectorPixelOS(vecScreen, colorValues[i]);
                                        // 倒车轨迹线不用标识具体距离
                                        // double fontX, fontY;
                                        // if (j == minYs)
                                        // {
                                        //         fontX = vecScreen[0] + 20;
                                        //         fontY = vecScreen[1] - 15;
                                        // }
                                        // else if (j + 0.5 > maxYs)
                                        // {   
                                        //         if(i == len - 1)
                                        //         {
                                        //                 fontX = vecScreen[0] - 80;
                                        //         }
                                        //         else
                                        //         {
                                        //                 fontX = vecScreen[0] - 50;
                                        //         }
                                        //         fontY = vecScreen[1] - 15;
                                        // }
                                        // if ((j == minYs || j + 0.5 > maxYs) && 
                                        // (fontX <= screenParams1.viewMaxX - 24 && fontY <= screenParams1.viewMaxY - 20 && fontX >= screenParams1.viewMinX + 20 && fontY >= screenParams1.viewMinY + 20))
                                        // {
                                        //         LCDDisplayASCII8X16(fontX, fontY, colorValues[i], wenzi[i]); 
                                        // }
                                }
                        }
                }
        }
}

void TrajectoryBack(double angle)
{
        //车辆后摄像头经过左右翻转
        angle = -angle;
        //如果角度值小于阈值,则认为是0度
        #if 1
        if (fabs(angle) < cameraParams1.angleThreshold)
        {
                angle = 0;
        }
        #endif
        //因为需要用到isMirror,放到了DrawTrajectory函数中
        int isMirror = 1;
        if (angle < 0)
        {
                isMirror = -1;
                angle =fabs(angle);
        }
        InitParamMatrix_Hou(angle, isMirror);
        // double limitPos[] = {0, 0, 0, 0};
        DrawHorizontalBackLine(angle, isMirror);
        DrawTrajectoryBack(angle, isMirror);
        // FreeParamMatrix();
}

//铲斗姿态
int chandou_index(unsigned short value){

	int chandou_value;
	int index;
	if(value < 500 || value > 4500){
		printf("error to read chandou value : %u\n", value);
		// return -1;
                return 0;
	}

	// chandou_value = (value-500)*45;
        // chandou_value = value*45;

	// if(chandou_value <= 30000)
	// 	chandou_value = 30000;
	// if(chandou_value >=150000)
	// 	chandou_value = 150000;

	// index = (chandou_value - 30000 ) / 2500;

        //<0
        float k0 = (2797 - 3771) * 1.0 / (0 - 18);
        float c0 = 2797;

        //>0
        float k1 = (3771 - 4125) * 1.0 / (18 - 41);
        float c1 = 4125 - 41 * k1;

        if (value <= 3771)
        {
                index =  (value - c0) / k0;
        }
        else if (value > 3771)
        {
                index =  (value - c1) / k1;
        }
       
       if (index < 0)
       {
                index = 0;
       }
       else if (index >= 42)
       {
                index = 41;
       }

	return  index;
}

int dongbi_index(unsigned short value){
	int chandou_value;
        int index;
        if(value < 500 || value > 4500){
                printf("error to read dongbi value : %u\n", value);
                // return -1;
                return 0;
        }

        // // chandou_value = (value-500)*45;
        // chandou_value = value*45;

        // if(chandou_value <= 90000)  //0   
        //         chandou_value = 90000;
        // if(chandou_value >=150000)  //60
        //         chandou_value = 150000;


        // index = (chandou_value - 90000 ) / 5000;

        
        // double k0 = (pointY0 - pointY1) / (pointX0 - pointX1);
        // double c0 = pointY1 - pointX1 * k0;

        float k = (970 - 2986) * 1.0 / (0 - 19);//155.077;//
        float c = 2986 - 19 * k;//969.999;//

        index = (value - c) / k;
       
       if (index < 0)
       {
                index = 0;
       }
       else if (index >= 19)
       {
                index = 18;
       }

        return  index;
}

void DrawCross()
{
        // float cameraParams0.width = 1920, cameraParams0.height = 1080;
        // double centerValue[] = {cameraParams0.width / 3, cameraParams0.height / 3};
        // Vector *center = InitVector(center, 2);
        // ValueVector(center, centerValue);
        // float lenFactor = 1.0f / 2;
        // for (float i = center->data[0] - lenFactor * cameraParams0.width; i < center->data[0] + lenFactor * cameraParams0.width; i+= 0.5f)
        // {
        //         double vecValue[] = {i, center->data[1]};
        //         Vector *vector = InitVector(vector, 2);
        //         ValueVector(vector, vecValue);
        //         SetVectorPixelOS(vector, RED);
        //         FreeVector(vector);
        // }
        // for (float i = center->data[1] - lenFactor * cameraParams0.height; i < center->data[1] + lenFactor * cameraParams0.height; i += 0.5f)
        // {
        //         double vecValue[] = {center->data[0], i};
        //         Vector *vector = InitVector(vector, 2);
        //         ValueVector(vector, vecValue);
        //         SetVectorPixelOS(vector, RED);
        //         FreeVector(vector);
        // }
        // for (float i = center->data[0] - 10; i < center->data[0] + 10; i += 0.5f)
        // {
        //         {
        //                 double vecValue[] = {i, center->data[1] + (1.0f / 10) * cameraParams0.height};
        //                 Vector *vector = InitVector(vector, 2);
        //                 ValueVector(vector, vecValue);
        //                 SetVectorPixelOS(vector, GREEN);
        //                 FreeVector(vector);
        //         }
        //         {
        //                 double vecValue[] = {i, center->data[1] - (1.0f / 10) * cameraParams0.height};
        //                 Vector *vector = InitVector(vector, 2);
        //                 ValueVector(vector, vecValue);
        //                 SetVectorPixelOS(vector, GREEN);
        //                 FreeVector(vector);
        //         }
        // }
        // for (float i = center->data[1] - 10; i < center->data[1] + 10; i += 0.5f)
        // {
        //         {
        //                 double vecValue[] = {center->data[0] + (1.0f / 10) * cameraParams0.width, i};
        //                 Vector *vector = InitVector(vector, 2);
        //                 ValueVector(vector, vecValue);
        //                 SetVectorPixelOS(vector, GREEN);
        //                 FreeVector(vector);
        //         }
        //         {
        //                 double vecValue[] = {center->data[0] - (1.0f / 10) * cameraParams0.width, i};
        //                 Vector *vector = InitVector(vector, 2);
        //                 ValueVector(vector, vecValue);
        //                 SetVectorPixelOS(vector, GREEN);
        //                 FreeVector(vector);
        //         }
        // }
}

// 设置当前距离
void SetDistance(uint32_t *fb_base0, unsigned char data0, unsigned char data1, float curAngle)
{
        // printf("data0 : %x, data1 : %x --------------data0 : %d, data1 : %d\n", data0, data1, data0, data1);
        int offsetXCenter = screenParams0.viewMinX + (screenParams0.viewMaxX - screenParams0.viewMinX) / 2;
        //收到距离数据0xFFFF时表示正在运输,不显示具体值
        if (data0 == 0xFF && data1 == 0xFF)
        {
                distance_f = DISTANCE_MAX;
                // blt_clr(fb_base0, cameraParams0.axisX - 40 - curAngle * 6, screenParams0.viewMaxY - 42, cameraParams0.axisX - 40 - curAngle * 6 + 110, screenParams0.viewMaxY - 42 + 50);
                // blt_clr(fb_base0, cameraParams0.axisX - 80 + curAngle * 6, screenParams0.viewMaxY - 40, cameraParams0.axisX - 80 + curAngle * 6 + 150, screenParams0.viewMaxY - 40 + 40);
                blt_clr(fb_base0, offsetXCenter - 75, screenParams0.viewMaxY - 40, offsetXCenter + 75, screenParams0.viewMaxY);
        }
        else
        {
                // 距离显示数值
                int distance0 = data0;
                int distance1 = data1;
                int distance = distance0 < distance1 ? distance0 : distance1;
                // 距离显示数值修正
                float distance_temp = ((float)distance) * 0.1;
                // if (fabs(distance_temp - distance_f) > 0.1 || distance_clear == 1)
                if (fabs(distance_temp - distance_f) > 0.09)
                {
                        // distance_clear = 0;
                        distance_f = distance_temp;
                        ShowDistance(fb_base0, curAngle);
                }
        }
}

// 显示距离
void ShowDistance(uint32_t *fb_base0, float curAngle)
{
        if (distance_f == DISTANCE_MAX)
        {
                return;
        }
        int offsetXCenter = screenParams0.viewMinX + (screenParams0.viewMaxX - screenParams0.viewMinX) / 2;
        // printf("offsetXCenter : %d\n", offsetXCenter);
        if (distance_f > distance_max)
        {
                char disStr[20];
                sprintf(disStr, ">%.1fM", distance_max);
                //75 = 150 / 2
                fill_text_area(offsetXCenter - 75, screenParams0.viewMaxY - 40, 150, 40, WHITE);
                LCDDisplayASCII24x36(offsetXCenter - 75,  screenParams0.viewMaxY - 40, GREEN, disStr);
                // fill_text_area(cameraParams0.axisX - 70 + curAngle * 2, screenParams0.viewMaxY - 40, 150, 40, WHITE);
                // LCDDisplayASCII24x36(cameraParams0.axisX - 70 + curAngle * 2,  screenParams0.viewMaxY - 40, GREEN, disStr);
        }
        else
        {
                char disStr[20];
                sprintf(disStr, "%.1fM", distance_f);
                fill_text_area(offsetXCenter - 75, screenParams0.viewMaxY - 40, 150, 40, WHITE);
                // fill_text_area(cameraParams0.axisX - 70 + curAngle * 2, screenParams0.viewMaxY - 40, 150, 40, WHITE);
                if (distance_f < 10)
                {
                        LCDDisplayASCII24x36(offsetXCenter - 50,  screenParams0.viewMaxY - 40, RED, disStr);
                        // LCDDisplayASCII24x36(cameraParams0.axisX - 50 + curAngle * 2,  screenParams0.viewMaxY - 40, RED, disStr);
                }
                else
                {
                        LCDDisplayASCII24x36(offsetXCenter - 60, screenParams0.viewMaxY - 40, RED, disStr);
                        // LCDDisplayASCII24x36(cameraParams0.axisX - 60 + curAngle *2,  screenParams0.viewMaxY - 40, RED, disStr);
                }
                // fill_text_area(cameraParams0.axisX - 60 + curAngle * 2, screenParams0.viewMaxY - 40, 150, 40, WHITE);
                // LCDDisplayASCII24x36(cameraParams0.axisX - 60 + curAngle * 2,  screenParams0.viewMaxY - 40, RED, disStr);
        }
}

void ShowAngleInScreen(uint32_t *fb_base0, double angle)
{
        char angleStr[20];
        int clear_x = 100;
        int clear_y = 50;
        #if IS_SHOW_SCREEN_TIME
        long time = getTime();
        sprintf(angleStr, "%ld : %.1fD", time, angle);
        clear_x = 300;
        clear_y = 50;
        #else
        sprintf(angleStr, "%.1fD", angle);
        clear_x = 100;
        clear_y = 50;
        #endif
        blt_clr(fb_base0, cameraParams0.axisX, screenParams0.viewMaxY + 40, cameraParams0.axisX + clear_x, screenParams0.viewMaxY + 40 + clear_y);
        LCDDisplayASCII8X16(cameraParams0.axisX, screenParams0.viewMaxY + 40 , RED, angleStr);
}

//是否画固定辅助横线
#if IS_DRAW_FIXED_AUXILIARY_HORIZONTAL_LINE
void DrawFixedAuxiliaryHorizontalLine()
{
        int line_center_x = 745;
        int line_center_y = 690;
        int line_length = 400;
        for (int i = line_center_x - line_length / 2; i < line_center_x + line_length / 2; i += 4)
        {
                SetPointPixelOS(i, line_center_y, RED);
        }
}
#endif

#if IS_SHOW_LIUGONG_LOGO
void DrawLogo(uint32_t *fb_base0)
{
        unsigned char logoLeft[30];
        unsigned char logoRight[30];

	// struct RECT_D logoLeft_rect = LOGO_PNG[0].pos_wh;
	// struct RECT_D logoRight_rect = LOGO_PNG[1].pos_wh;

	// sprintf(logoLeft, "/tmp/logo/%s", LOGO_PNG[0].filename);
        // printf("logoLeft : %d , value = %s\n", strlen(logoLeft), logoLeft);
	// sprintf(logoRight, "/tmp/logo/%s", LOGO_PNG[1].filename);
        // printf("logoRight : %d , value = %s\n", strlen(logoRight), logoRight);

        // struct filename_postion filename[]={
	// 	{ logoLeft_rect.x, logoLeft_rect.y, logoLeft },
	// 	{ logoRight_rect.x, logoRight_rect.y, logoRight }
	// };
	// png_blend(filename,sizeof(filename)/sizeof(struct filename_postion));

        int logo_fd = open("/tmp/bmp/x8.bmp", O_RDONLY, S_IRUSR);
        fill_bmp_base(logo_fd, (uint32_t *)(fb_base0),  0, 266, 322, 457);
	close(logo_fd);

        // struct filename_postion filename[]={
	// 	{ 314, 94, "/tmp/logo/ttt.png" }
	// };
	// png_blend(filename, 1);
} 
#endif

void InitConfig()
{
        get_parameter();
        InitParameter();
        set_screen_line_parameter();
        // printf("InitConfig \n");
}

int main(int argc, char *argv[])
{
	uint32_t *fb_base0;
	uint32_t *fb_base1;
	uint32_t *fb_ctrl;
	int i;
	int alpha;
 	int logo_fd;
	char disp_str[128];
	struct display_param param;
	int frame0;
	int fd_param;
	int *buffer_param;
	int fd_video;
	int *buffer_video;

	char freq[10];
	char dbm1[5];
	char dbm2[5];
	int flag_nosignal;
	int no_signal = 0;

	int freq_value;	

	int x0;
	int y0;

	int n_read;
	unsigned char _rx_data[16];
	int angle;
	float angle_f = 0.0;
	float angle_f_pre = 0.0;
	
	int distance0, distance1;
        float distance_pre = DISTANCE_MAX;

	int index_chandou=0;
	int index_chandou_pre = -1;

	
	int index_dongbi=0;
	int index_dongbi_pre = -1;

        int isTestZero = 1;

	fb_base0 = (int32_t *)p2v(0x11800000, 1920*1080*4);
	printf("fb_base virt 0x%08x\n", (uint32_t)fb_base0);
	alpha = atoi(argv[1]);

	memset(fb_base0,0x00,1920*1080*4);
	fb_base1 = (int32_t *)p2v(0x12100000, 1920*1080*4);
        printf("fb_base virt 0x%08x\n", (uint32_t)fb_base1);
	
	memset(fb_base1,0x00,1920*1080*4);
	fb_ctrl = (int32_t *)p2v(0x43c00000, 4096);

	i = 0;

	fd_video = shm_open("/video.txt",O_RDWR|O_CREAT|O_EXCL,0777);
        if(fd_video < 0){
               fd_video = shm_open("/video.txt",O_RDWR,0777);
        }else
                ftruncate(fd_video,4096);
        buffer_video= mmap(NULL,4096,PROT_READ|PROT_WRITE,MAP_SHARED,fd_video,0);


	fb_ctrl[56] = 0x5;
	
	working_fb=fb_base0;
	g_pLCDBuffer = fb_base0;

        #if IS_INITCONFIG_AT_START
        InitConfig();
        #endif

        //本机测试可以打开，否则程序会等待Can信号无法调试
        #if 1

        buffer_video[4] = 2;
	double W0=atof(argv[2]);
	int W1=atof(argv[3]);
	int W2=atof(argv[4]);
        blt_clr(fb_base0,0,0,1920,1080);

        #if IS_SHOW_LIUGONG_LOGO
        DrawLogo(fb_base0);
        // png_display(fb_base0, base_x, base_y);
        while(1)
        {
                usleep(frameRate);
        }
        #endif

        int isLoopTest = 0;
        if (isLoopTest)
        {
                W0 = -36;
        }
        while(1)
        {
                #if !IS_INITCONFIG_AT_START
                InitConfig();
                #endif
                // blt_clr(fb_base0,screenParams0.viewMinX - 4, screenParams0.viewMinY - 4, screenParams0.viewMaxX + 4, screenParams0.viewMaxY + 4);
                // blt_clr(fb_base0,640,320,1280,800);
                // double R1 = GetR1(W0);
                // printf("W0=%lf,R=%lf\n", W0, R1);
                // printf("W0 :::::: %lf\n", W0);
                // render(W0,640+190+240,700,1);
                // render(W0,640+170,700,0);

                blt_clr(fb_base0, screenParams0.viewMinX, screenParams0.viewMinY, screenParams0.viewMinX + screenParams0.viewMaxX - screenParams0.viewMinX, screenParams0.viewMinY + screenParams0.viewMaxY - screenParams0.viewMinY);
                Trajectory(-W0+reviseAngle);
                //在屏幕上显示度数
                if (1)
                {
                        ShowAngleInScreen(fb_base0, W0);
                }

                //测试显示后倒车轨迹线
                if (0)
                {
                        blt_clr(fb_base0, screenParams1.viewMinX, screenParams1.viewMinY, screenParams0.viewMaxX, screenParams1.viewMaxY);
                        TrajectoryBack(-W0+reviseAngle);
                }
                
                //测试显示距离
                if (0)
                {
                        unsigned char _rx_data[16] = {0x00,0x00,0x00,0x00,0x00,0x00,0x00,
                                                                    0x00,0x00,0x00,0x00,0x00,0x00,0x00,
                                                                    W1, W2};
                        SetDistance(fb_base0, _rx_data[14], _rx_data[15], W0);
                        ShowDistance(fb_base0, W0);
                        W1++;
                        W2++;
                }

                //是否画固定辅助横线
                if (1)
                {
                        #if IS_DRAW_FIXED_AUXILIARY_HORIZONTAL_LINE
                        DrawFixedAuxiliaryHorizontalLine();
                        #endif
                }
                
                // printTime();

                if (isLoopTest)
                {
                        W0 = W0 + 1;
                        if (W0 > 36.0) 
                        {
                                W0 = W0 - 36.0 * 2.0;
                        }
		        usleep(frameRate);
                }
                else
                {
                        break;
                }
        }

        FreeParameter();
        return 0;
	#endif
	
	#if 0
        n_read = uart_init(ROUTE_UART, "/dev/ttyPS0", 115200, 'N');
        if(n_read <=0 ) {
                printf("open /dev/ttyPS0 failed\n");
                return -1;
        }

        // int timeCount = 0;

	// while(timeCount < 100) {

        //         timeCount++;
        //         usleep(frameRate);
        // }
        // printf("beginbeginbeginbeginbegin\n");
        n_read = uart_get(ROUTE_UART, _rx_data);

	while(1) {

                n_read = uart_get(ROUTE_UART, _rx_data);

                //ShowAngleInScreen(fb_base0, n_read);

                if(n_read == 16)
                {
                        ShowAngleInScreen(fb_base0, _rx_data[15]);
                }

                usleep(frameRate);
        }
	#endif

        //测试画十字准心
        #if 0
        while(1)
        {
                #if !IS_INITCONFIG_AT_START
                InitConfig();
                #endif
                blt_clr(fb_base0, screenParams0.viewMinX, screenParams0.viewMinY, screenParams0.viewMinX + screenParams0.viewMaxX - screenParams0.viewMinX, screenParams0.viewMinY + screenParams0.viewMaxY - screenParams0.viewMinY);
                DrawCross();
                usleep(20000);
        }
        #endif

        //测试png
        #if 0
        //chandou : 2797~3771~4125
        int index_chandou_in = 2797;
        //dongbi : 970~2986
        int index_dongbi_in = 970;

        printf("index_chandou_in : %d, index_dongbi_in : %d\n", index_chandou_in, index_dongbi_in);

        distance_f = atof(argv[2]);

        while(1)
        {
                while(1)
                {
                        int index_chandou_temp = chandou_index(index_chandou_in);
                        // printf("index_chandou_temp : %d\n", index_chandou_temp);
                        int index_dongbi_temp  = dongbi_index(index_dongbi_in);
                        // printf("index_dongbi_temp : %d\n", index_dongbi_temp);
                        #if !IS_INITCONFIG_AT_START
                        InitConfig();
                        #endif
                        //在屏幕上显示图片ID
                        #if 1
                        char str[30];
                        sprintf(str, "CD:%d       DB:%d", index_chandou_temp, index_dongbi_temp);
                        LCDDisplayASCII8X16(1500, 60 , RED, str);
                        #endif

                        DRAW_Base_Bkt_Bgr(index_chandou_temp, index_dongbi_temp, distance_f);
                        png_display(fb_base0, base_x, base_y);
                        ShowCDZTDistanceValue(distance_f);
                        // usleep(20000*10);
                        printTime();
                        usleep(frameRate);
                        // usleep(10);
                        #if 1
                        index_chandou_in += 30;
                        if (index_chandou_in >= 4125)
                        {
                                index_chandou_in = 2797;
                                break;
                        }
                        // if (index_chandou_temp % 2 == 0)
                        // {
                        //         index_chandou_temp -= 2;
                        // }
                        // if (index_chandou_temp % 2 == 1)
                        // {
                        //         index_chandou_temp += 2;
                        // }
                        // if (index_chandou_temp == -2)
                        // {
                        //         index_chandou_temp = 1;
                        // }
                        // else if (index_chandou_temp == 43)
                        // {
                        //         break;
                        // }
                        #endif
                }
                #if 1
                index_dongbi_in += 106;
                if (index_dongbi_in >= 2986)
                {
                        index_dongbi_in = 970;
                }
                // index_chandou_temp = 42;
                // index_dongbi_temp += 1;
                // if (index_dongbi_temp > 14)
                // {
                //         index_dongbi_temp = 0;
                // }
                usleep(frameRate);
                #endif
        }
        #endif

        #if IS_COM_TO_CAN_SIM
	n_read = uart_init(ROUTE_UART, "/dev/ttyPS0", 115200, 'N');
        #else
	n_read = uart_init(ROUTE_UART, "/dev/ttyUL0", 115200, 'N');
        #endif
        if(n_read <=0 ) {
                #if IS_COM_TO_CAN_SIM
                printf("open /dev/ttyPS0 failed\n");
                #else
                printf("open /dev/ttyUL0 failed\n");
                #endif
                return -1;
        }

        n_read = uart_get(ROUTE_UART, _rx_data);

	while(1) {

                #if !IS_INITCONFIG_AT_START
                InitConfig();
                #endif

		flag_nosignal = buffer_video[0];

                #if IS_USE_TEST_TESTING
                //注意这句也会控制轨迹线的显示隐藏
                // if (!(flag_nosignal == 1 && no_signal == 0))
                // {
                //         flag_nosignal = no_signal = 1;
                // }
                flag_nosignal = 1;
                no_signal = 0;

                ISSHOWTRACKLINE = ISSHOWDISTANCE = ISSHOWTRACKLINE_AFTER = ISSHOWBUCKET = testParams.testIsShowTrackline;
                // printf("ISSHOWTRACKLINE : %d,    ISSHOWDISTANCE : %d,    ISSHOWTRACKLINE_AFTER : %d,    ISSHOWBUCKET : %d\n", ISSHOWTRACKLINE, ISSHOWDISTANCE, ISSHOWTRACKLINE_AFTER, ISSHOWBUCKET);
                if (testParams.testIsShowTrackline == 0)
                {
                        blt_clr(fb_base0,0,0,1920,1080);
                }
                angle_f = testParams.testAngle;
                #endif

		n_read = uart_get(ROUTE_UART, _rx_data);
		if(n_read == 16 && _rx_data[4] == 0x18 && _rx_data[5] == 0xFF && _rx_data[6] == 0x20 && _rx_data[7] == 0xE4)
                // if(n_read == 16 && _rx_data[4] == 0x0C && _rx_data[5] == 0xFF && _rx_data[6] == 0x02 && _rx_data[7] == 0xE4)
		{	
			// printf("find CAN ID 0x18FF20E4\n");
                        // #if ISSHOWTRACKLINE
                        if (ISSHOWTRACKLINE == 1)
                        {
                                //输出角度传感器原始数据
                                // printf("UART_READ  14::%d,15::%d\n", _rx_data[14], _rx_data[15]);
                                #if 1
                                printf("  14::%d,,", _rx_data[14]);
                                printf("  15::%d,,", _rx_data[15]);
                                // printf("   14 &&::%d,,", (_rx_data[14] >> 5) & 0x7);
                                // printf("   15 &&::%d,,", _rx_data[15]<<3);
                                // printf("   angle::%d,,", (( (_rx_data[14] >> 5) & 0x7) ) | (_rx_data[15]<<3));
                                
                                // angle = (((_rx_data[14] << 5) & 0x7FF)) | (_rx_data[15]) ;
                                #endif
                                angle = (((_rx_data[14] >> 5) & 0x7)) | (_rx_data[15] << 3) ;
                                printf("      angle::%d,,", angle);

                                float angle_temp = ((float)(angle-900))*0.1;
                                angle_temp = angle_temp + 21.0f;
                                printf("      angle_temp::%f,,\n", angle_temp);
                                
                                //输出角度传感器原始数据转换值
                                #if 0
                                printf("angle_temp :::::: %f\n", angle_temp);
                                #endif
                                // if (angle_temp != angle_f)
                                // {
                                //         printf("angle_temp :::::: %f\n", angle_temp);
                                // }
                                angle_f = angle_temp;
                        }
                        // #endif

                        //铲斗姿态
			// #if ISSHOWBUCKET
                        if (ISSHOWBUCKET == 1)
                        {
                                unsigned short chandou_value = (_rx_data[11] << 8) | _rx_data[10];
                                index_chandou = chandou_index(chandou_value);
                                
                                unsigned short dongbi_value = (_rx_data[9] << 8) | _rx_data[8];	

                                index_dongbi  = dongbi_index(dongbi_value);
                                
                                #if 0
                                printf("chandou_value %u  dongbi_value %u \n",chandou_value,dongbi_value);
                                printf("index chandou %d  index dongbi %d \n",index_chandou,index_dongbi);
                                #endif
                        }
                        // #endif
			//铲斗姿态

		} 
                else if (n_read == 16 && _rx_data[4] == 0x18 && _rx_data[5] == 0xFF && _rx_data[6] == 0x25 && _rx_data[7] == 0xE4)
                {
			// printf("find CAN ID 0x18FF25E4\n");
                        // 毫米波雷达距离显示
			// #if ISSHOWDISTANCE
                        if (ISSHOWDISTANCE == 1)
                        {
                                #if 0
                                printf("   distance::%d,%d\n", _rx_data[14], _rx_data[15]);
                                #endif
                                float curAngle = -angle_f+reviseAngle;
                                //收到距离数据0xFFFF时表示正在运输,不显示具体值
                                SetDistance(fb_base0, _rx_data[14], _rx_data[15], curAngle);
                        }
			// #endif
		} 
                else if (n_read == 16 &&  _rx_data[4] == 0x0C && _rx_data[5] == 0xFF && _rx_data[6] == 0x1D && _rx_data[7] == 0x4C)
                {
			printf("find CAN ID 0x0CFF1D4C\n");
			if (1) 
                        {
                                printf("1D4C [9]::%d ,, ", _rx_data[9]);
                                printf("%d ,, ", (_rx_data[9] & 0x02) >> 1);
                                printf("%d\n", _rx_data[9] & 0x01);
                        }

                        // int show_line_flag = (_rx_data[9] & 0x02) >> 1;
                        // int show_line_after_flag = _rx_data[9] & 0x01;
                        // if (ISSHOWTRACKLINE != show_line_flag)
                        // {
                        //         ISSHOWTRACKLINE = show_line_flag;
                        // }
                        // if (ISSHOWTRACKLINE_AFTER != show_line_after_flag)
                        // {
                        //         ISSHOWTRACKLINE_AFTER = show_line_after_flag;
                        // }
		} 
                else if (n_read == 16 &&  _rx_data[4] == 0x0C && _rx_data[5] == 0xFF && _rx_data[6] == 0x1B && _rx_data[7] == 0x4C)
                {
			// printf("find CAN ID 0x0CFF1B4C\n");
			buffer_video[4] = _rx_data[10]	;
		} 
                // 2024.4.17-yanda
                // else if (n_read == 16 &&  _rx_data[4] == 0xFF && _rx_data[5] == 0xFF && _rx_data[6] == 0xFF && _rx_data[7] == 0xFF) 
                // {
                //         // printf("find CAN ID 0xFFFFFFFF\n");
                //         if (_rx_data[15] == 0x00)
                //         {
                //                 ISSHOWTRACKLINE = ISSHOWDISTANCE = ISSHOWTRACKLINE_AFTER = ISSHOWBUCKET = 0;
                //         }
                //         else
                //         {
                //                 ISSHOWTRACKLINE = ISSHOWDISTANCE = ISSHOWTRACKLINE_AFTER = ISSHOWBUCKET = 1;
                //         }
                //         if (ISSHOWTRACKLINE == 0)
                //         {
                //                 blt_clr(fb_base0,0,0,1920,1080);
                //         }
                //         // angle_f = testParams.testAngle;
                // }

                //加入进行无can信号模拟调试
                #if 1
                buffer_video[4] = 2;
                #endif

        // angle_f = -angle_f;

	#if 1	
		if((flag_nosignal==0) && (no_signal ==0)){
			logo_fd = open("/tmp/nosignal.bmp", O_RDONLY, S_IRUSR);
			fill_bmp(logo_fd, (uint32_t *)(fb_base0), alpha, 1920, 1080);
			close(logo_fd);
			no_signal = 1;
		}else if((flag_nosignal == 1) && (no_signal == 1)){
			blt_clr(fb_base0,0,0,1920,1080);
                        printf("blt_clr\n\n");
			no_signal =0 ;

                        //是否显示铲斗姿态
                        // #if ISSHOWBUCKET
                        if (ISSHOWBUCKET == 1)
                        {
                                DRAW_Base_Bkt_Bgr(index_chandou,index_dongbi, distance_f);
                                png_display(fb_base0, base_x, base_y);
                                ShowCDZTDistanceValue(distance_f);
                        }
                        // #endif
		}
                //是否显示轨迹线
		// #if ISSHOWTRACKLINE
                if (ISSHOWTRACKLINE == 1)
                {
                        // if (isTestZero == 1)
                        // {
                        //         Trajectory(0);
                        //         isTestZero = 0;
                        // }

                        // if(no_signal == 0){
                        #if ISNOANTIFLASHING
                        if(no_signal == 0 && fabs(angle_f_pre - angle_f) > 0.2)
                        #else
                        if(no_signal == 0 && (angle_f != angle_f_pre))
                        #endif
                        {
                                if (angle_f > -38 && angle_f <38)
                                {
                                        // render(angle_f,640+190+240,700,1);
                                        // render(angle_f,640+170,700,0);

                                        // printf("Draw :  %f\n",-angle_f+reviseAngle);
                                        // blt_clr(fb_base0, screenParams0.viewMinX - 4, screenParams0.viewMinY - 4, screenParams0.viewMaxX + 4, screenParams0.viewMaxY + 4);

                                        // blt_clr(fb_base0, 0, 0, cameraParams0.width, cameraParams0.height);
                                        blt_clr(fb_base0, screenParams0.viewMinX, screenParams0.viewMinY, screenParams0.viewMinX + screenParams0.viewMaxX - screenParams0.viewMinX, screenParams0.viewMinY + screenParams0.viewMaxY - screenParams0.viewMinY);
                                        // distance_clear = 1;
                                        angle_f_pre = angle_f;
                                        //画轨迹线
                                        #if 1
                                        if (ISSHOWTRACKLINE == 1)
                                        {
                                                Trajectory(-angle_f+reviseAngle);
                                        //Trajectory(angle_f+reviseAngle);
                                        }
                                        #endif

                                        //毫米波雷达距离显示,如果清除了且距离数值无变化需要显示
                                        // #if ISSHOWDISTANCE
                                        if (ISSHOWDISTANCE == 1)
                                        {
                                                float curAngle = -angle_f+reviseAngle;
                                                ShowDistance(fb_base0, curAngle);
                                        }
                                        // #endif

                                        //画九宫格下方车子后面直线
                                        // #if ISSHOWTRACKLINE_AFTER
                                        if (ISSHOWTRACKLINE_AFTER == 1)
                                        {
                                                blt_clr(fb_base0, screenParams1.viewMinX, screenParams1.viewMinY, screenParams0.viewMaxX, screenParams1.viewMaxY);
                                                TrajectoryBack(-angle_f+reviseAngle);
                                        }
                                        // #endif

                                        //是否画固定辅助横线
                                        #if IS_DRAW_FIXED_AUXILIARY_HORIZONTAL_LINE
                                        DrawFixedAuxiliaryHorizontalLine();
                                        #endif

                                        //在屏幕上显示度数
                                        #if 0
                                        ShowAngleInScreen(fb_base0, -angle_f+reviseAngle);
                                        #endif

                                        //画十字准心
                                        #if 0
                                        DrawCross();
                                        #endif
                                }
                        }
                }
		// #endif

                //是否显示铲斗姿态
                // #if ISSHOWBUCKET
                if (ISSHOWBUCKET == 1)
                {
                        if(no_signal == 0 ){  // chan dou && dong bi
                                if((index_chandou != index_chandou_pre) || (index_dongbi != index_dongbi_pre)
                                        || (fabs(distance_pre - distance_f) > 0.1))
                                {
                                        // printf("Draw Chandou : index_chandou : %d, index_dongbi : %d \n", index_chandou, index_dongbi);
                                        index_chandou_pre = index_chandou;
                                        index_dongbi_pre = index_dongbi;
                                        distance_pre = distance_f;
                                        //姿态画前是否清除原来的
                                        #if 0
                                        blt_clr(fb_base0, base_x, base_y, base_x + base_w, base_y + base_h);
                                        #endif

                                        DRAW_Base_Bkt_Bgr(index_chandou,index_dongbi, distance_f);
                                        png_display(fb_base0, base_x, base_y);
                                        ShowCDZTDistanceValue(distance_f);
                                        //在屏幕上显示图片ID
                                        #if 0
                                        char str[30];
                                        sprintf(str, "CD:%d       DB:%d", index_chandou, index_dongbi);
                                        LCDDisplayASCII8X16(1500, 60 , RED, str);
                                        #endif
                                }	
                                //DRAW_Base_Bkt_Bgr(0,0);
                        }
                }
		// #endif

	#endif
                //1s(秒)=1000ms(毫秒)=1000000us(微秒)
		usleep(frameRate);                     //2000us(微秒,百万分之一秒)=2ms
                // usleep(1000000);        //1秒
	}

        // FreeParameter();
        // printf("FreeParameter \n");

	return 0;
}
